def execute(self):
if self.runs_ == -1: # run forever
self.child_status_ = self.children_[0].execute()
if self.child_status_ == 'SUCCESS':
rospy.logwarn('REPEAT DECORATOR ['+self.name_+']: SUCCEEDED, RESET')
self.children_[0].reset()
return self.set_status('RUNNING')
elif self.child_status_ == 'RUNNING':
return self.set_status('RUNNING')
elif self.child_status_ == 'FAILURE':
rospy.logwarn('REPEAT DECORATOR ['+self.name_+']: FAILED, RESET')
self.children_[0].reset()
return self.set_status('RUNNING')
else:
### FIX ME ###
if self.num_runs_ < self.runs_:
self.child_status_ = self.children_[0].execute()
if self.child_status_ == 'SUCCESS':
self.num_runs_ += 1
elif self.child_status_ == 'RUNNING':
return self.set_status('RUNNING')
elif self.child_status_ == 'FAILURE':
return self.set_status('FAILURE')
else:
return self.set_status('SUCCESS')
def open_alarm(self, event):
'''React when an alarm has been clicked
1. Use event to determine which cell has been clicked
2. Select that cell's whole row
3. Use the cached json_descriptions of each alarm to get that row's json data
When an alarm has been clicked, we'll display its json parameters in the box on the right
'''
items_selected = self.alarm_table.selectedItems()
if len(items_selected) == 0:
return
row_selected = items_selected[0].row()
self.alarm_table.selectRow(row_selected)
key_item = self.alarm_table.itemAt(row_selected, 0)
try:
json_parameters = json.loads(self.alarm_parameter_cache[key_item])
except ValueError:
rospy.logwarn("Could not decode alarm message")
return
self.alarm_tree.clear()
try:
build_tree_from_json(self.alarm_tree, json_parameters)
except AssertionError:
rospy.logwarn("Could not draw json tree for alarm (Is it a dictionary?)")
def attach(self, object_name, actuate = True):
if actuate:
# attach the collision object to the gripper
self.gripper_close.call()
# get the actual collision obj from planning scene
remove_object = None
rospy.logwarn('Attaching object: %s'% object_name)
res = self.get_planning_scene(components=
PlanningSceneComponents(components=PlanningSceneComponents.WORLD_OBJECT_GEOMETRY))
all_objects = res.scene.world.collision_objects
for col_obj in all_objects:
if col_obj.id == object_name:
remove_object = col_obj
break
# self.planning_group.attachObject(object_name, self.end_link)
if remove_object is not None:
self.planning_scene_publisher = rospy.Publisher('planning_scene', PlanningScene, queue_size = 10)
planning_scene_diff = PlanningScene(is_diff=True)
remove_object.operation = CollisionObject.REMOVE
del planning_scene_diff.world.collision_objects[:];
planning_scene_diff.world.collision_objects.append(copy.deepcopy(remove_object));
attached_object = AttachedCollisionObject()
attached_object.object = remove_object
attached_object.link_name = self.end_link
attached_object.object.header.frame_id = self.joint_names[-1]
attached_object.object.operation = CollisionObject.ADD
del planning_scene_diff.robot_state.attached_collision_objects[:];
planning_scene_diff.robot_state.attached_collision_objects.append(attached_object);
self.planning_scene_publisher.publish(planning_scene_diff)
def imu_remap(imu_msg):
global namespace
# global noisy
# remap imu message to base_link_self and publish the new message
remapped_msg = imu_msg
remapped_msg.header.frame_id = namespace + 'base_link_filter'
# if noisy:
# # We will add noise to the IMU data
# noise = randn(7, 1)
#
# orientation = remapped_msg.orientation
# noisy_yaw = convert_quaternion_to_yaw(orientation) + noise[0]
# orientation = convert_yaw_to_quaternion(noisy_yaw)
#
# angular_velocity = remapped_msg.angular_velocity
# angular_velocity.x += noise[1]
# angular_velocity.y += noise[2]
# angular_velocity.z += noise[3]
#
# linear_acceleration = remapped_msg.linear_acceleration
# linear_acceleration.x += noise[4]
# linear_acceleration.y += noise[5]
# linear_acceleration.z += noise[6]
try:
imu_publisher.publish(remapped_msg)
except rospy.ROSException as e:
rospy.logwarn(e.message)
def update_subs(self, topics):
for topic in topics:
# Different elements publish different topic types
topic_type = String
cb = self.string_cb
name,kind = topic
if kind == GUIElement.BUTTON_ID:
topic_type = Int32
cb = self.int_cb
elif kind == GUIElement.BUTTONGROUP_ID:
topic_type = String
cb = self.string_cb
elif kind == GUIElement.STOPWATCH_ID:
topic_type = Duration
cb = self.dur_cb
elif kind == GUIElement.INTSLIDER_ID:
topic_type = Int32
cb = self.int_cb
else:
rospy.logwarn("Unsupported element type.")
continue
if name.strip('/') not in self.subs:
print "Subscribing to topic: " + name
rospy.Subscriber(name, topic_type, cb)
# reset info on the topic, even if already subscribed
self.subs[name.strip('/')] = {"type_id":kind,
"pressed":False,
"last_press": ""}
def smart_move_cb(self,req):
'''
Find any valid object that meets the requirements.
Find a cartesian path or possibly longer path through joint space.
'''
if not self.driver_status == 'SERVO':
msg = 'FAILURE -- not in servo mode'
rospy.logerr('DRIVER -- Not in servo mode!')
return msg
# Check acceleration and velocity limits
(acceleration, velocity) = self.check_req_speed_params(req)
possible_goals = self.query(req)
stamp = self.acquire()
if len(possible_goals) == 0:
return 'FAILURE -- no valid poses found. see costar_arm.py'
for (dist,T,obj,name) in possible_goals:
rospy.logwarn("Trying to move to frame at distance %f"%(dist))
# plan to T
if not self.valid_verify(stamp):
msg = 'FAILURE - Stopping smart move action because robot has been preempted by another process. see costar_arm.py'
rospy.logwarn(msg)
return msg
(code,res) = self.planner.getPlan(T,self.q0,obj=obj)
msg = self.send_and_publish_planning_result(res,stamp,acceleration,velocity)
if msg[0:7] == 'SUCCESS':
break
return msg
def servo_to_joints_cb(self,req):
if self.driver_status == 'SERVO':
# Check acceleration and velocity limits
(acceleration, velocity) = self.check_req_speed_params(req)
stamp = self.acquire()
# inverse kinematics
traj = self.planner.getJointMove(req.target.position,
self.q0,
self.base_steps,
self.steps_per_meter,
self.steps_per_radians,
time_multiplier = (1./velocity),
percent_acc = acceleration,
use_joint_move = True,
table_frame = self.table_pose)
# Send command
if len(traj.points) > 0:
if stamp is not None:
rospy.logwarn("Robot moving to " + str(traj.points[-1].positions))
res = self.send_trajectory(traj,stamp,acceleration,velocity,cartesian=False)
self.release()
else:
res = 'FAILURE -- could not preempt current arm control.'
return res
else:
rospy.logerr('SIMPLE DRIVER -- IK failed')
return 'FAILURE -- no trajectory points'
else:
rospy.logerr('SIMPLE DRIVER -- Not in servo mode')
return 'FAILURE -- not in servo mode'
def executeBodyPose(self, goal):
if not goal.pose_name in self.poseLibrary:
rospy.loginfo("Pose %s not in library, reload library from parameters..." % goal.pose_name)
self.readInPoses()
if goal.pose_name in self.poseLibrary:
rospy.loginfo("Executing body pose %s...", goal.pose_name);
if not self.stopWalkSrv is None:
self.stopWalkSrv()
trajGoal = JointTrajectoryGoal()
# time out one sec. after trajectory ended:
trajGoal.trajectory = self.poseLibrary[goal.pose_name]
timeout = trajGoal.trajectory.points[-1].time_from_start + Duration(1.0)
trajGoal.trajectory.header.stamp = rospy.Time.now()
# TODO: cancel goal after timeout is not working yet in nao_controller
self.trajectoryClient.send_goal_and_wait(trajGoal, timeout)
if self.trajectoryClient.get_state() != GoalStatus.SUCCEEDED:
self.poseServer.set_aborted(text="JointTrajectory action did not succeed (timeout?)");
self.poseServer.set_succeeded()
rospy.loginfo("Done.");
else:
msg = "pose \""+goal.pose_name+"\" not in pose_manager's library";
rospy.logwarn("%s", msg)
self.poseServer.set_aborted(text=str(msg));
def __init__(self):
# ROS initialization:
rospy.init_node('pose_manager')
self.poseLibrary = dict()
self.readInPoses()
self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
execute_cb=self.executeBodyPose,
auto_start=False)
self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
try:
rospy.wait_for_service("stop_walk_srv", timeout=2.0)
self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
except:
rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
+"This is normal if there is no nao_walker running.")
self.stopWalkSrv = None
self.poseServer.start()
rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());
else:
rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
rospy.signal_shutdown("Required component missing");
def Loop(self):
rospy.loginfo("Arlobot Calibration Node entering loop")
while not rospy.is_shutdown():
self._loop_rate.sleep()
else:
rospy.logwarn("Arlobot Base Node: shutdown invoked, exiting")
def checkVer(self, input):
data = []
gotHeaderStart = False
verInfo = VersionResponds()
input.setTimeout(1)
countUntilTimeout = 0
while not rospy.is_shutdown() and countUntilTimeout < 3:
try:
if gotHeaderStart:
for i in range(1, 11):
data.append(input.read())
verInfo.buildRequest(data)
if verInfo.checkPackage() and abs(verInfo.getVersion() - VERSION) < 1:
if verInfo.getVersion() < VERSION:
rospy.logwarn(
"RiCBord has a firmware %.2f please update the firmware for better performers" % (
verInfo.getVersion()))
elif verInfo.getVersion() > VERSION:
rospy.logwarn(
"RiCBord has a firmware %.2f please update your package for better performers" % (
verInfo.getVersion()))
return True
else:
return False
elif ord(input.read()) == HEADER_START:
gotHeaderStart = True
except TypeError:
pass
finally:
countUntilTimeout += 1
return False
def parse(node, verbose=True):
joint = Joint(node.getAttribute('name'), None, None,
node.getAttribute('type'))
for child in children(node):
if child.localName == 'parent':
joint.parent = child.getAttribute('link')
elif child.localName == 'child':
joint.child = child.getAttribute('link')
elif child.localName == 'axis':
joint.axis = child.getAttribute('xyz')
elif child.localName == 'origin':
joint.origin = Pose.parse(child)
elif child.localName == 'limit':
joint.limits = JointLimit.parse(child)
elif child.localName == 'dynamics':
joint.dynamics = Dynamics.parse(child)
elif child.localName == 'safety_controller':
joint.safety = SafetyController.parse(child)
elif child.localName == 'calibration':
joint.calibration = JointCalibration.parse(child)
elif child.localName == 'mimic':
joint.mimic = JointMimic.parse(child)
else:
if verbose:
rospy.logwarn("Unknown joint element '%s'"%child.localName)
return joint
def execute(self, userdata):
rospy.loginfo(OKGREEN+"i'm looking what tags are"+ENDC)
rospy.loginfo(OKGREEN+str(userdata.asr_userSaid)+ENDC)
#rospy.loginfo(OKGREEN+"TAGS: "+str(self.tags)+ENDC)
#rospy.loginfo(OKGREEN+str(userdata.asr_userSaid_tags)+ENDC)
#userdata.object=userdata.asr_userSaid # it means that in this place it have a coke
if userdata.asr_userSaid=="finish" :
rospy.logwarn("-------------------------------------i'm have a finish order")
return 'finish'
listTags = userdata.asr_userSaid_tags
#Process tags
userdata.object_array = []
locationValue = [tag for tag in listTags if tag.key == 'direction']
objectValue = [tag for tag in listTags if tag.key == 'class']
if objectValue and locationValue:
userdata.objectOrientation = locationValue[0].value
userdata.objectName = objectValue[0].value
return 'new_position'
else:
rospy.logerr("Object or Location not set")
return 'aborted'
def execute(self, userdata):
rospy.logwarn("i'm looking for::::::::::::::. "+str (userdata.name))
# first of all i look if it's some faces
if userdata.faces.faces:
# i look in what option we are, if we are looking for a name o no
if userdata.name!="":
userdata.face=[face for face in userdata.faces.faces if face.name==userdata.name]
if userdata.face:
userdata.face=userdata.face.pop()
userdata.standard_error="Recognize_face_Name OK"+userdata.standard_error
userdata.face_frame = userdata.faces.header.frame_id
rospy.loginfo('-- FRAME ID:: ' + userdata.faces.header.frame_id)
self.request_preempt()
return 'succeeded'
else :
userdata.standard_error="Recognize:= Any face whit that name"+userdata.standard_error
return 'aborted'
# if we are no looking for a face we will organize
else:
# i want to take the best face confidence
userdata.faces.faces.sort(cmp=None, key=attrgetter('confidence'), reverse=True)
userdata.face=userdata.faces.faces[0]
userdata.face_frame = userdata.faces.header.frame_id
rospy.loginfo('-- FRAME ID:: ' + userdata.faces.header.frame_id)
userdata.standard_error="Recognize_face_Normal OK"+userdata.standard_error
self.request_preempt()
return 'succeeded'
else:
userdata.standard_error="no faces available"+userdata.standard_error
userdata.face=None
return 'aborted'
def obtain_masters(self):
'''
This method use the service 'list_masters' of the master_discoverer to get
the list of discovered ROS master. Based on this list the L{SyncThread} for
synchronization will be created.
@see: U{master_discovery_fkie.interface_finder.get_listmaster_service()
<http://docs.ros.org/api/master_discovery_fkie/html/modules.html#interface-finder-module>}
'''
if not rospy.is_shutdown():
service_names = interface_finder.get_listmaster_service(masteruri_from_master(), False)
for service_name in service_names:
try:
with self.__lock:
try:
socket.setdefaulttimeout(5)
discoverMasters = rospy.ServiceProxy(service_name, DiscoverMasters)
resp = discoverMasters()
masters = []
master_names = [m.name for m in resp.masters]
rospy.loginfo("ROS masters obtained from '%s': %s", service_name, master_names)
for m in resp.masters:
if self._can_sync(m.name): # do not sync to the master, if it is in ignore list or not in filled sync list
masters.append(m.name)
self.update_master(m.name, m.uri, m.timestamp, m.timestamp_local, m.discoverer_name, m.monitoruri, m.online)
for key in set(self.masters.keys()) - set(masters):
self.remove_master(self.masters[key].name)
except rospy.ServiceException, e:
rospy.logwarn("ERROR Service call 'list_masters' failed: %s", str(e))
except:
import traceback
rospy.logwarn("ERROR while initial list masters: %s", traceback.format_exc())
finally:
socket.setdefaulttimeout(None)
self.update_timer = threading.Timer(self.UPDATE_INTERVALL, self.obtain_masters)
self.update_timer.start()
def fit_joints_to_range(self, qvec):
self.qvecIK = qvec
fits = True
does_fit = False
for i in range(6):
#print "qvec", qvec
#q = qvec[i]
#treat d3 differently
if (i!=2):
does_fit = self.fit_q_to_range(q_lower_limits[i],q_upper_limits[i],i)
#special case for d3...although generic formula also works in this case
else:
does_fit = True
if (self.qvecIK[i] < q_lower_limits[i]):
does_fit = False
if (self.qvecIK[i] > q_upper_limits[i]):
does_fit = False
if (not does_fit):
if(debug_needle_print):
rospy.logwarn("IK err: jnt %d lower lim: %f upper lim: %f desired val = %f"%(i,q_lower_limits[i],q_upper_limits[i],self.qvecIK))
#qvec[i] = q
fits = fits and does_fit
if (fits):
return True
else:
return False
def __init__(self, config_path, bus_layout):
'''Thruster driver, an object for commanding all of the sub's thrusters
- Gather configuration data and make it available to other nodes
- Instantiate ThrusterPorts, (Either simulated or real), for communicating with thrusters
- Track a thrust_dict, which maps thruster names to the appropriate port
- Given a command message, route that command to the appropriate port/thruster
- Send a thruster status message describing the status of the particular thruster
'''
self.alarm_broadcaster = AlarmBroadcaster()
self.thruster_out_alarm = self.alarm_broadcaster.add_alarm(
name='thruster_out',
action_required=True,
severity=3
)
self.failed_thrusters = []
self.make_fake = rospy.get_param('simulate', False)
if self.make_fake:
rospy.logwarn("Running fake thrusters for simulation, based on parameter '/simulate'")
# Individual thruster configuration data
newtons, thruster_input = self.load_config(config_path)
self.interpolate = scipy.interpolate.interp1d(newtons, thruster_input)
# Bus configuration
self.port_dict = self.load_bus_layout(bus_layout)
thrust_service = rospy.Service('thrusters/thruster_range', ThrusterInfo, self.get_thruster_info)
self.thrust_sub = rospy.Subscriber('thrusters/thrust', Thrust, self.thrust_cb, queue_size=1)
self.status_pub = rospy.Publisher('thrusters/thruster_status', ThrusterStatus, queue_size=8)
# This is essentially only for testing
self.fail_thruster_server = rospy.Service('fail_thruster', FailThruster, self.fail_thruster)
def reset_iris_neutral_value(self):
if self.dic_sequence_services['checked_sequence_of_missions'] == True:
new_service = {}
trigger_instant = self._widget.TriggerInstantNeutral.value()
new_service['trigger_instant'] = trigger_instant
self.dic_sequence_services['last_trigger_time'] = trigger_instant
new_service['service_name'] = 'IrisPlusResetNeutral'
new_service['inputs_service'] = {}
self.dic_sequence_services['list_sequence_services'].append(new_service)
else:
try:
# time out of one second for waiting for service
rospy.wait_for_service(self.namespace+'IrisPlusResetNeutral',1.0)
try:
# request reseting neutral value for iris+ (implicit, with keywords)
ResetingNeutral = rospy.ServiceProxy(self.namespace+'IrisPlusResetNeutral', IrisPlusResetNeutral)
reply = ResetingNeutral()
if reply.received == True:
self._widget.ThrottleNeutralValue.setValue(reply.k_trottle_neutral)
rospy.logwarn('Service for reseting neutral value succeded')
except rospy.ServiceException as exc:
rospy.logwarn("Service did not process request: " + str(exc))
rospy.logwarn('Service for reseting neutral value failed')
pass
except rospy.ServiceException as exc:
rospy.logwarn("Service did not process request: " + str(exc))
rospy.logwarn('Service for reseting neutral value failed')
pass
def tts_speak(text, wait_before_speaking=0):
"""
Lets the robot say the given text out aloud. This is a blocking call.
"""
from text_to_speech.msg import SoundAction, SoundGoal
global __tts_client
if __tts_client is None:
__tts_client = actionlib.SimpleActionClient(TTS_ACTION_NAME, SoundAction)
rospy.logdebug('Waiting for "%s"...' % TTS_ACTION_NAME)
if not __tts_client.wait_for_server(rospy.Duration(1.0)):
rospy.logwarn('Couldn\'t connect to "%s" server.' % TTS_ACTION_NAME)
return False
tts_goal = SoundGoal()
tts_goal.wait_before_speaking = rospy.Duration(wait_before_speaking)
tts_goal.text = text
rospy.logdebug('Sent speak command with "%s" (wait: %.3f seconds).' % (
text, wait_before_speaking))
__tts_client.send_goal(tts_goal)
__tts_client.wait_for_result()
result = __tts_client.get_state()
rospy.logdebug('Result for last speech command: %s' % result)
return result == GoalStatus.SUCCEEDED
def __init__(self):
# Start node
rospy.init_node("recognizer_en")
self._device_name_param = "~mic_name" # Find the name of your microphone by typing pacmd list-sources in the terminal
self._lm_param = "~lm"
self._dic_param = "~dict"
# Configure mics with gstreamer launch config
if rospy.has_param(self._device_name_param):
self.device_name = rospy.get_param(self._device_name_param)
self.device_index = self.pulse_index_from_name(self.device_name)
self.launch_config = "pulsesrc device=" + str(self.device_index)
rospy.loginfo("Using: pulsesrc device=%s name=%s", self.device_index, self.device_name)
else:
self.launch_config = 'gconfaudiosrc'
rospy.loginfo("Launch config: %s", self.launch_config)
self.launch_config += " ! audioconvert ! audioresample " \
+ '! vader name=vad auto-threshold=true ' \
+ '! pocketsphinx name=asr ! fakesink'
# + '! pocketsphinx hmm=en_US/hub4wsj_sc_8k name=asr ! fakesink'
# Configure ROS settings
self.started = False
rospy.on_shutdown(self.shutdown)
self.pub = rospy.Publisher('~output', String)
rospy.Service("~start", Empty, self.start)
rospy.Service("~stop", Empty, self.stop)
if rospy.has_param(self._lm_param) and rospy.has_param(self._dic_param):
self.start_recognizer()
else:
rospy.logwarn("lm dic and hmm parameters need to be set to start recognizer.")
def send_list_of_services(self):
# # debug
# for service in self.dic_sequence_services['list_sequence_services']:
# print(service)
# print('\n\n')
# request service
try:
# time out of one second for waiting for service
rospy.wait_for_service(self.namespace+'ServiceSequencer',1.0)
try:
request = rospy.ServiceProxy(self.namespace+'ServiceSequencer', ServiceSequence)
service_sequence = json.dumps(self.dic_sequence_services['list_sequence_services'])
reply = request(service_sequence = service_sequence)
if reply.received == True:
# if controller receives message
print('Success')
except rospy.ServiceException as exc:
rospy.logwarn("Service did not process request: " + str(exc))
except rospy.ServiceException as exc:
rospy.logwarn("Service did not process request: " + str(exc))
def __init__(self):
rospy.init_node('twist_converter', anonymous=True)
self.num_robots=int(rospy.get_param('~num_robots',1))
self.publishers = [None]*self.num_robots;
self.subscribers = [None]*self.num_robots;
if rospy.has_param('~robot_prefix'): #if there is a robot prefix assume that there is actually one or more
#full_param_name = rospy.search_param('robot_prefix')
#robot_prefix = rospy.get_param(full_param_name)
robot_prefix=rospy.get_param('~robot_prefix')
for r in range(self.num_robots):
self.publishers[r]=rospy.Publisher(robot_prefix+str(r)+'/cmd_vel',Twist);
self.subscribers[r] = rospy.Subscriber(robot_prefix+str(r)+'/image', Image, self.callback, r)
else: # if no robot prefix, assume that there is only one robot
self.publishers[0] = rospy.Publisher('cmd_vel',Twist);rospy.logwarn("assuming /cmd_vel, number of robots actually "
+str(self.num_robots))
self.subscribers[0] = rospy.Subscriber("image", Image, self.callback, 0)
self.data_uri = rospy.get_param("data_uri","/twist");
self.urls = (self.data_uri,'twist', "/stop","stop","/controller","controller")
self.data = ['-10']*self.num_robots;
self.datasize = [0]*self.num_robots;
self.port=int(rospy.get_param("~port","8080"));
#self.data_uri2 = rospy.get_param("data_uri","/pose");
rospy.logwarn("running")
def UnArming_Quad(self,base_name=""):
#This function is used to arm the quad."""
#Un-Arming the Quad
srv_path = self.namespace+'mavros/cmd/arming'
# if base_name!="":
# srv_path = "/%s/%s"%(base_name,srv_path)
try:
rospy.logwarn('Un-Arming Quad ...')
rospy.wait_for_service(srv_path,2.0)
try:
arming = rospy.ServiceProxy(srv_path,CommandBool)
arming_result=arming(False)
if arming_result.success:
rospy.logwarn('Quad is Un-Armed!!!!')
# return True
else:
rospy.logwarn('Cannot Un-arm quad')
# return False
except:
rospy.logwarn('Cannot Un-arm quad')
# return False
except:
rospy.logwarn('No connection to Mavros')
def setAngle(self, turn):
"set wheel angle"
# Try various functions with domain [-1..1] to improve
# sensitivity while retaining sign. At higher speeds, it may
# make sense to limit the range, avoiding too-sharp turns and
# providing better control sensitivity.
#turn = math.pow(turn, 3) * ArtVehicle.max_steer_radians
#enqueues first 5 data points based on the first movements on the wheel
if(self.counter < 5):
self.counter +=1
self.steeringQueue.put(self.steering.angle/180)
else:
#dequeue oldest value
oldVal = self.steeringQueue.get()
#get rate of change of angle by subtracting final(turn) - initial(oldVal) and dividing by .25s which is time it takes to get 5 values
currentRate = (self.steering.angle/180 - oldVal)/.25
limit = 2.0361 * math.pow(.72898, self.pilot.pstate.current.speed)
rospy.logwarn(str(currentRate) + ' ' + str(limit))
#if currentRate is above limit, restrict currentRate to the limit at the target velocity, else if it's at or below limit don't do anything
#get new value of turn by using equation below:
#(final angle - initial angle)/change time = limit; angle final = limit * change in time + angle inital
if(math.fabs(currentRate > limit)):
rospy.logwarn('Success')
turn = limit*.25 + oldVal
#put new steering angle into queue
self.steeringQueue.put(turn)
# ensure maximum wheel angle never exceeded
self.pilot.steer(turn)
def execute(self):
if self.display_name is not None:
running_service = '%s [%s]'%(self.service_description,self.display_name)
else:
running_service = '%s [%s]'%(self.service_description,self.name_)
if self.needs_reset:
rospy.loginfo('%s already [%s], needs reset'%(running_service,self.get_status()))
return self.get_status()
else:
rospy.logwarn("running? " + str(self.running) + "; " + str(self.finished_with_success))
if not self.running: # Thread is not running
if self.finished_with_success is None: # Service was never called
try:
self.service_thread.start()
rospy.loginfo('%s running'%running_service)
self.running = True
return self.set_status('RUNNING')
except Exception, errtxt:
self.status_msg = '%s thread failed'%running_service
rospy.logwarn(self.status_msg)
self.running = False
self.needs_reset = True
rospy.logerr('failed -- %s'%self.status_msg)
return self.set_status('FAILURE -- %s'%self.status_msg)
else:# If thread is running
def ref_generation():
xbox_ctrl = rospy.Publisher('xboxctrl', numpy_msg(Floats))
rospy.init_node('xboxbridge', anonymous=True)
rate = rospy.Rate(100) # Run no faster than 100hz
UDP_IP = "192.168.10.81"
UDP_PORT = 27300
sock = socket.socket(socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
sock.bind((UDP_IP, UDP_PORT))
sock.setblocking(0)
packer = struct.Struct('c c c c f f f f B B h')
while not rospy.is_shutdown():
try:
data = sock.recv(48) # buffer size is 48 bytes
if len(data) == 24:
if data[0] == "X" and data[1] == "B" and data[2] == "O" and data[3] == "X":
output = packer.unpack(data)
ctrl = numpy.array([output[4],output[5],output[6],output[7],output[8]], dtype=numpy.float32)
xbox_ctrl.publish(ctrl)
#print output
except:
# print "Nothing"
if str(sys.exc_info()[1]) != '[Errno 11] Resource temporarily unavailable':
rospy.logwarn("Unexpected error: %s", sys.exc_info()[1])
nothing = 0
rate.sleep()
def _getSSH(self, host, user, pw=None, do_connect=True):
'''
@return: the paramiko ssh client
@rtype: L{paramiko.SSHClient}
@raise BadHostKeyException: - if the server's host key could not be verified
@raise AuthenticationException: - if authentication failed
@raise SSHException: - if there was any other error connecting or establishing an SSH session
@raise socket.error: - if a socket error occurred while connecting
'''
session = SSHhandler.SSH_SESSIONS.get(host, paramiko.SSHClient())
if session is None:
t = SSHhandler.SSH_SESSIONS.pop(host)
del t
session = SSHhandler.SSH_SESSIONS.get(host, paramiko.SSHClient())
if session._transport is None:
session.set_missing_host_key_policy(paramiko.AutoAddPolicy())
while (session.get_transport() is None or not session.get_transport().authenticated) and do_connect:
try:
session.connect(host, username=user, password=pw, timeout=3)
except Exception, e:
# import traceback
# print traceback.format_exc()
if str(e) in ['Authentication failed.', 'No authentication methods available']:
res, user, pw = self._requestPW(user, host)
if not res:
return None
self.SSH_AUTH[host] = user
else:
rospy.logwarn("ssh connection to %s failed: %s", host, str(e))
return None
else:
SSHhandler.SSH_SESSIONS[host] = session
if not session.get_transport() is None:
session.get_transport().set_keepalive(10)
def _update(self, event):
if self.robot and self.traj:
now = time.time()
if (now - self.traj_t0) <= self.traj.points[-1].time_from_start.to_sec():
setpoint = sample_traj(self.traj, now - self.traj_t0)
try:
self.robot.send_servoj(999, setpoint.positions, 4 * self.RATE)
except socket.error:
pass
else: # Off the end
if self.goal_handle:
last_point = self.traj.points[-1]
state = self.robot.get_joint_states()
position_in_tol = within_tolerance(state.position, last_point.positions, self.joint_goal_tolerances)
velocity_in_tol = within_tolerance(state.velocity, last_point.velocities, [0.005]*6)
if position_in_tol and velocity_in_tol:
# The arm reached the goal (and isn't moving). Succeeding
self.goal_handle.set_succeeded()
self.goal_handle = None
elif now - (self.traj_t0 + last_point.time_from_start.to_sec()) > self.goal_time_tolerance.to_sec():
# Took too long to reach the goal. Aborting
rospy.logwarn("Took too long to reach the goal.\nDesired: %s\nactual: %s\nvelocity: %s" % \
(last_point.positions, state.position, state.velocity))
self.goal_handle.set_aborted(text="Took too long to reach the goal")
self.goal_handle = None
def on_automode_message(self, msg):
if msg.data == True: # if autonomous mode requested
if self.state == self.STATE_IDLE:
if self.wptnav.pose != False: # if we have a valid pose
(num_wpt, next_wpt) = self.wptlist.status()
if num_wpt > 0:
self.state = self.STATE_NAVIGATE
if self.wptnav.state == self.wptnav.STATE_STANDBY:
self.wptnav.resume()
rospy.loginfo(rospy.get_name() + ": Resuming waypoint navigation")
elif self.wptnav.state == self.wptnav.STATE_STOP:
self.goto_first_wpt()
rospy.loginfo(rospy.get_name() + ": Switching to waypoint navigation")
else:
if self.no_route_plan_warn == False:
rospy.logwarn(rospy.get_name() + ": No route plan available")
self.no_route_plan_warn = True
else: # no valid pose yet
if self.automode_warn == False:
self.automode_warn = True
rospy.logwarn(rospy.get_name() + ": Absolute pose is required for autonomous navigation")
else: # if manual mode requested
if self.state != self.STATE_IDLE:
self.state = self.STATE_IDLE
self.wptnav.standby()
rospy.loginfo(rospy.get_name() + ": Switching to manual control")
def callback(self,msg,id):
#get the data from the message and store as a string
try:
self.data[id] = msg.data;
self.datasize[id]=msg.width*msg.height*4; #assumes rgba8
except Exception, err:
rospy.logwarn("Cannot convert the Image message due to %s, with robot %s" % err, id)
'the pointcloud message topic to be subscribed, default `/kitti/points_raw`',
default='/kitti/points_raw')
parser.add_argument(
'--pub_topic',
type=str,
help=
'the pointcloud message topic to be published, default `/squeeze_seg/points`',
default='/squeeze_seg/points')
parser.add_argument(
'--pub_feature_map_topic',
type=str,
help=
'the 2D spherical surface image message topic to be published, default `/squeeze_seg/feature_map`',
default='/squeeze_seg/feature_map')
parser.add_argument(
'--pub_label_map_topic',
type=str,
help=
'the corresponding ground truth label image message topic to be published, default `/squeeze_seg/label_map`',
default='/squeeze_seg/label_map')
args = parser.parse_args()
rospy.init_node('segment_node')
node = SegmentNode(sub_topic=args.sub_topic,
pub_topic=args.pub_topic,
pub_feature_map_topic=args.pub_feature_map_topic,
pub_label_map_topic=args.pub_label_map_topic,
FLAGS=FLAGS)
rospy.logwarn("finished.")
def _compute_reward(self, observations, done):
reward = 0
if not done:
# Cost for uncertainty in tracking and check if target is still visible
# self.uncertainty_cost(observations, epsilon = -2, lower_bound = -4)
# Only if target is in sight and visible
# if not self.lost_target_flag:
# '''Heading Reward'''
# [flag,desired_heading] = self.is_in_desired_heading(observations,epsilon=0.5)
# if flag:
# reward+=0.16*self.detection_data
# else:
# reward+=0.0
'''Following reward in world workspace.'''
# [flag,distance_from_target] = self.is_in_desired_distance(observations,epsilon_up=7,epsilon_down=3.5)
[flag,
distance_from_target] = self.is_in_desired_distance(observations,
epsilon=0.5)
if flag:
reward += 0.25 * self.detection_data
else:
reward += 0.01 * self.detection_data
# Following height reward in world workspace
[flag, height] = self.is_in_MAV_desired_altitude(observations,
epsilon=0.5)
if flag:
reward += 0.25 * self.detection_data
else:
reward += 0.01 * self.detection_data
# '''Following velocity reward in world workspace'''
# [flag,vel_err] = self.is_in_desired_velocity(observations, epsilon=0.1)
# if flag:
# reward += 0.2*self.detection.data
# else:
# reward+= 0.01*self.detection.data
# '''Perception centering reward'''
# [flag,distance_to_imgcenter] = self.is_image_centered(observations, epsilon=0.1)
# if flag:
# reward += 0.16*self.detection_data
# else:
# reward += 0.01*self.detection_data
# reward+= -1*(observations[3])*10*distance_to_imgcenter**2 -1*(1-observations[3])*10 + 10
# reward+= -1*distance_to_imgcenter**2 + 1
# '''Perception height cost / Tracking reward in image space'''
# [flag,height_ratio] = self.is_desired_height(observations, epsilon=0.05)
# if flag:
# reward += 0.16*self.detection_data*(1-self.half_visible)
# else:
# reward += 0.01*self.detection_data*(1-self.half_visible)
# reward += 10*detection.data*(-1*(height_ratio-self.desired_height_ratio)**2 + 0.5)
# if flag:
# reward += observations[3]*self.desired_height_reward
# else:
# reward +=0
# reward += -1*(observations[3])*20*(height_ratio-self.desired_height_ratio)**2 - 1*(1-observations[3])*10 + 10
'''Minimize person velocity in image plane'''
[flag, flow] = self.is_flow_zero(observations, epsilon=0.5)
if flag:
reward += 0.25 * self.detection.data
else:
reward += 0.01 * self.detection.data
#
'''Penalize Workspace Violation'''
# if self._MAV_out_ws:
# reward -= 0.5
# Perception joint cost / Joint tracking cost in image space
# joint_r= self.joint_detections_reward(observations, epsilon=0.5)
# reward+=self.joint_detections_reward_gain*joint_r
# If there has been a decrease in the distance towards target, we reward it
[flag, rad_distance_from_target
] = self.is_in_desired_rad_distance(observations,
epsilon_up=7,
epsilon_down=3.5)
delta_error = rad_distance_from_target - self.previous_distance
self.previous_distance = rad_distance_from_target
if delta_error < 0.0 and rad_distance_from_target > np.sqrt(
self.desired_distance**2 + self.desired_distance**2) + 0.5:
reward += 0.25 * self.detection_data
rospy.logwarn(
"################### DECREASING ERROR: GOOD JOB ###########################"
)
else:
reward += 0.0
target = self.get_gt_target()
# rospy.logerr("@@@@ 1. d2img_center: " + str(distance_to_imgcenter))
rospy.logerr("@@@@ 2. MAV altitude : " + str(height))
# rospy.logerr("@@@@ 3. flow: " + str(flow))
# rospy.logerr("@@@@ 3. h_ratio: " + str(height_ratio)+" image_height: "+str(self.image_height))
rospy.logerr("@@@@ 4. d2t: " + str(distance_from_target))
# rospy.logerr("@@@@ 5. velocity err: " + str(vel_err))
rospy.logerr("@@@@ 6. joint_obs: " + str(observations[5:]))
# rospy.logerr("@@@@ 4. joint_reward: " + str(self.joint_detections_reward_gain*joint_r))
# rospy.logerr("@@@@ 7. sum(joint_probabilities)/17: " + str(sum(observations[10:])/17))
# rospy.logerr("@@@@ 8. detection: " + str(observations[3]))
'''
else:
if self.is_in_desired_position():
reward += self.surface_reward
else:distance_to_imgcenter/self.max_distance_to_imgcenter
reward += -1*self.surface_reward
'''
# else:
# if self.cumulated_steps != self.step_limit:
# reward+=-20*(self.step_limit - self.cumulated_steps)
if reward >= 0.8:
rospy.logerr("")
rospy.logerr(
"@@@@@@@@@@@@@@@@@@@@@@@@ AWESOME AGENT @@@@@@@@@@@@@@@@@@@@")
rospy.logerr("")
rospy.logwarn("reward=" + str(reward))
rospy.logdebug("reward=" + str(reward))
self.reward_msg.data = reward
self._reward_pub.publish(self.reward_msg)
self.cumulated_reward += reward
rospy.logdebug("Cumulated_reward=" + str(self.cumulated_reward))
self.cumulated_steps += 1
rospy.logdebug("Cumulated_steps=" + str(self.cumulated_steps))
return reward
def _get_obs(self):
"""
Read robot state
:return:
"""
rospy.logdebug("Start Get Observation ==>")
# We get the pose of the MAV
# pose = self.get_gt_odom()
# target = self.get_target()
# target_velocity = self.get_target_velocity()
# feedback = self.get_detections_feedback()
alphapose = self.get_alphapose()
bbox = self.get_alphapose_bbox()
joint_detections = np.array(alphapose.res).reshape((17, 3))
bbox_coordinates = np.array(bbox.bbox)
detection = self.get_alphapose_detection()
# self.uncertainty_cost([], epsilon = -2, lower_bound = -4)
# if self.lost_target_flag:
# detection = 0
# else:
# detection = 1
# normalizing features
joint_detections[:, 0] = [
float(joint_detections[k, 0] / self.image_width)
for k in range(len(joint_detections[:, 0]))
]
joint_detections[:, 1] = [
float(joint_detections[k, 1] / self.image_height)
for k in range(len(joint_detections[:, 1]))
]
joints_flattened = joint_detections[:, 0:2].reshape(34, )
# joint_probabilities = joint_detections[:,2]
# We round to only two decimals to avoid very big Observation space position, velocity of robot, position and velocity of target,
# center of image, head and feet position
if bbox_coordinates[0] <= 2 or bbox_coordinates[
1] <= 2 or bbox_coordinates[
2] >= self.image_width - 2 or bbox_coordinates[
3] >= self.image_height - 2:
self.half_visible = 1
else:
self.half_visible = 0
xcenter = round((bbox_coordinates[2] - bbox_coordinates[0]) /
2) + bbox_coordinates[0]
ycenter = round((bbox_coordinates[3] - bbox_coordinates[1]) /
2) + bbox_coordinates[1]
observations = [\
float(detection.data*xcenter/self.image_width), float(detection.data*ycenter/self.image_height), \
float(detection.data*abs(bbox_coordinates[3]-bbox_coordinates[1])/self.image_height), \
float(detection.data*(xcenter/self.image_width - self.prev_center[0])), float(detection.data*(ycenter/self.image_height - self.prev_center[1]))
]
#+list(joints_flattened)
# detection.data, \
# abs(pose.position.z/15)
# # feedback.xcenter - self.prev_feedback.xcenter, \
# # feedback.ycenter - self.prev_feedback.ycenter \
#+ list(joint_probabilities)
# observations = list(joints_flattened) + [detection.data*xcenter/self.image_width, detection.data*ycenter/self.image_height, \
# detection.data*abs(bbox_coordinates[3]-bbox_coordinates[1])/self.image_height]
self.detection_data = detection.data
rospy.logwarn("Center==>" + str(xcenter) + " " + str(ycenter))
self.prev_center = [observations[0], observations[1]]
rospy.logwarn("Observations==>" + str(observations))
rospy.logdebug("Observations==>" + str(observations))
rospy.logdebug("END Get Observation ==>")
self.act_pretrained, _states = self.pretrained_model.predict(
observation=observations[0:5], deterministic=True)
return observations
def _check_all_systems_ready(self):
""" Wait for actor to respawn and move """
rospy.logwarn("Wait for actor to respawn and move")
rospy.sleep(5)
except (rospy.ServiceException, rospy.exceptions.ROSException), e:
rospy.logwarn(str(e))
rospy.logwarn("service exception from world_model")
# Get all missing objects
try:
rospy.wait_for_service(
'/env/asr_world_model/get_missing_object_list', timeout=5)
get_missing_object_list = rospy.ServiceProxy(
'/env/asr_world_model/get_missing_object_list',
GetMissingObjectList)
self.missing_pbd_typeAndId = get_missing_object_list(
).missingObjects
except (rospy.ServiceException, rospy.exceptions.ROSException), e:
rospy.logwarn(str(e))
rospy.logwarn("service exception from world_model")
return 'no_objects_found'
# We need to filter the search_objects in random/cropbox search mode
# cropbox record doesn't use object_detection
mode = rospy.get_param("/scene_exploration_sm/mode")
if mode == 4 or mode == 5:
self.searched_object_types = set()
for obj in self.missing_pbd_typeAndId:
self.searched_object_types.add(str(obj.type))
global numberOfObjectDetectionRuns
global numberOfAllSearchedObjectTypes
numberOfObjectDetectionRuns += 1
numberOfAllSearchedObjectTypes += len(self.searched_object_types)
cumulated_reward = 0
done = False
if qlearn.epsilon > 0.05:
qlearn.epsilon *= epsilon_discount
#print(str(x)+"=============="+str(len(qlearn.q))+"================================================================================")
# Initialize the environment and get first state of the robot
observation = env.reset()
state = ''.join(map(str, observation))
# Show on screen the actual situation of the robot
# env.render()
# for each episode, we test the robot for nsteps
#for i in range(nsteps):
i = 0
while (True):
rospy.logwarn("############### Start Step=>" + str(i))
# Pick an action based on the current state
action = qlearn.chooseAction(state)
rospy.logwarn("Next action is:%d", action)
# Execute the action in the environment and get feedback
observation, reward, done, info = env.step(action)
rospy.logwarn(str(observation) + " " + str(reward))
cumulated_reward += reward
if highest_reward < cumulated_reward:
highest_reward = cumulated_reward
nextState = ''.join(map(str, observation))
# Make the algorithm learn based on the results
rospy.logwarn("# state we were=>" + str(state))
def execute(self, userdata=None):
arm = self._arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed"
gravitation = 9.81
try_current = 0
measure_force = MeasureForce(self._robot)
# To be able to determine the weight of the object the difference between the default weight and the weight with
# the object needs to be taken. Note that initially the weight with the object is set to the default weight to
# be able to enter the while loop below.
arm_weight = measure_force.get_force()
arm_with_object_weight = arm_weight
weight_object = numpy.linalg.norm(numpy.subtract(arm_weight, arm_with_object_weight)) / gravitation
while weight_object < self._minimal_weight and try_current < self._try_num:
if try_current == 0:
self._robot.speech.speak("Let me try to pick up the garbage")
else:
self._robot.speech.speak("I failed to pick up the trash, let me try again")
rospy.loginfo("The weight I felt is %s", weight_object)
try_current += 1
# This opening and closing is to make sure that the gripper is empty and closed before measuring the forces
# It is necessary to close the gripper since the gripper is also closed at the final measurement
# arm.send_gripper_goal('open')
# arm.wait_for_motion_done()
# arm.send_gripper_goal('close', max_torque=1.0)
# arm.wait_for_motion_done()
arm_weight = measure_force.get_force()
rospy.loginfo("Empty weight %s", arm_weight)
# Open gripper
arm.send_gripper_goal('open')
arm.wait_for_motion_done()
# Go down and grab
try:
arm.move_down_until_force_sensor_edge_up(timeout=7)
except TimeOutException:
rospy.logwarn("No forces were felt, however no action is taken!")
pass
# self._robot.torso.send_goal("grab_trash_down")
# self._robot.torso.wait_for_motion_done()
# rospy.sleep(3)
arm.send_gripper_goal('close', max_torque=1.0)
arm.wait_for_motion_done()
# Go up and back to pre grasp position
self._robot.torso.send_goal("grab_trash_up")
self._robot.torso.wait_for_motion_done()
arm_with_object_weight = measure_force.get_force()
rospy.loginfo("Full weight %s", arm_with_object_weight)
weight_object = numpy.linalg.norm(numpy.subtract(arm_weight, arm_with_object_weight)) / gravitation
rospy.loginfo("weight_object = {}".format(weight_object))
# Go back and pull back arm
self._robot.head.look_up()
self._robot.head.wait_for_motion_done()
self._robot.base.force_drive(-0.5, 0, 0, 2.0)
arm.send_joint_goal('handover')
arm.wait_for_motion_done()
arm.send_joint_goal('reset')
arm.wait_for_motion_done()
self._robot.head.reset()
#
# if weight_object < self._minimal_weight:
# return "failed"
self._robot.speech.speak("Look at this I can pick up the trash!")
handed_entity = EntityInfo(id="trash")
arm.occupied_by = handed_entity
return "succeeded"
def update_action_state(self, string_msg):
if string_msg.data in self.allowed_action_state_list:
self.action_state = string_msg.data
else:
rospy.logwarn('update_action_state: \'%s\' value not allowed for action state' %(string_msg.data))
def create_osm_node_dict(xml_file_location):
'''
Receives:
xml_file_location:
a string with the location of the OSM file to be processed
Returns:
osm_node_dict:
a dictionary with all the nodes in the map
osm_tag_dict:
a dictionary with all the found tags, and it's corresponding node ids.
Each key of the fictionary corresponds to a tag, and the value of each
key is a list with the id/ids of node/s with this tag.
origin_node:
an OMSNode corresponding to the origin node
If the OSM Map does not define an origin node, it will return None
'''
XML_file = xml_file_location
tree = ET.parse(XML_file)
xml_root = tree.getroot()
osm_node_dict = dict()
osm_tag_dict = dict()
origin_node = None
# Dict of traffic light nodes to their series, which is a
# string denoting whether they are to be turned on or
# off at a timestep
osm_traffic_light_nodes = dict()
# dict of crosswalk nodes:
# the key is the crosswalk node,
# the value its id
osm_crosswalk_nodes = defaultdict(list)
for node in xml_root.findall('node'):
add_flag = True
for tag in node.findall('tag'):
node_id = int( node.get('id') )
origin_lat = float( node.get('lat') )
origin_lon = float( node.get('lon') )
if tag.get('k') == "origin":
if tag.get('v') == "true":
origin_node = RoadAuxClasses.OSMNode(node_id, origin_lat, origin_lon)
# This node should not be added to the node list
osm_tag_dict['origin'] = []
osm_tag_dict['origin'].append(node_id)
continue
elif tag.get('k') == 'traffic':
if tag.get('v') == 'light':
if tag.get('series') == None:
logwarn('Traffic light requires a series')
else:
osm_traffic_light_nodes[node_id] = tag.get('series')
elif tag.get('k') == 'crosswalk':
osm_crosswalk_nodes[int(tag.get('v'))].append(node_id)
else:
tag_string = tag.get('v')
special_node = RoadAuxClasses.OSMNode(node_id, origin_lat, origin_lon, tag = tag_string)
osm_node_dict[node_id] = special_node
if osm_tag_dict.has_key(tag_string):
osm_tag_dict[tag_string].append(node_id)
else:
osm_tag_dict[tag_string] = [node_id]
continue
node_id = int( node.get('id') )
lat = float( node.get('lat') )
lon = float( node.get('lon') )
osm_node_dict[node_id] = RoadAuxClasses.OSMNode(node_id, lat, lon)
return [osm_node_dict, osm_tag_dict, origin_node, osm_traffic_light_nodes, osm_crosswalk_nodes]
def update_system_state(self, string_msg):
if string_msg.data in self.allowed_system_state_list:
self.system_state = string_msg.data
else:
rospy.logwarn('update_system_state: \'%s\' value not allowed for system state' %(string_msg.data))
def run(self):
if self.active_mission_len > 0:
try:
if self.manual_mission.fsm_state == manual_mission.State.IDLE:
self.active_waypoint_gps = self.active_mission_gps[
self.active_mission_idx]
else:
self.active_waypoint_gps = self.manual_mission.mission[
self.manual_mission.mission_idx]
except IndexError as err:
rospy.logwarn(err)
rospy.logwarn(
"Can't assign active waypoint. Mission is not up to date yet."
)
if self.main_mode in PAUSE_LIST_MAIN or self.sub_mode in PAUSE_LIST_SUB:
# print("Setting paused!")
self.fsm_state = State.PAUSED
# ------------------------------------------------------------------------------ #
if self.fsm_state == State.GROUNDED:
if self.new_mission:
# prepare for upload and change state
self.active_mission_gps = self.pending_mission_gps
self.active_mission_ml = self.gps_to_mavlink(
self.active_mission_gps)
self.new_mission = False
self.fsm_state = State.CLEARING_MISSION
self.clear_mission_pub.publish(Empty())
# ------------------------------------------------------------------------------ #
elif self.fsm_state == State.REQUESTING_UPLOAD:
request = UploadMissionRequest(
drone_id=self.id, waypoints=self.active_mission_ml.waypoints)
response = self.upload_mission(request)
if response.success:
self.fsm_state = State.UPLOADING
self.upload_done = False
# ------------------------------------------------------------------------------ #
elif self.fsm_state == State.UPLOADING:
# only a positive ack counts as a succesful upload (see callback)
if self.upload_done:
if self.state == "Standby":
request = TakeoffDroneRequest(on_the_spot=True, alt=20)
response = self.takeoff(request)
if response.success:
self.fsm_state = State.TAKEOFF
elif self.state == "Active":
self.manual_mission.stop_running()
self.manual_mission.reset()
self.fsm_state = State.SYNC_WP_IDX
elif self.upload_failed:
rospy.Timer(rospy.Duration(UPLOAD_DELAY),
self.wait_cb,
oneshot=True)
self.wait = True
self.fsm_state = State.WAITING
self.upload_failed = False
# ------------------------------------------------------------------------------ #
elif self.fsm_state == State.TAKEOFF:
if self.sub_mode == "Takeoff":
response = self.arm()
if response.success:
self.fsm_state = State.ARMING
else:
if self.cmd_try_again:
request = TakeoffDroneRequest(on_the_spot=True, alt=20)
response = self.takeoff(request)
if response.success:
self.cmd_try_again = False
# ------------------------------------------------------------------------------ #
elif self.fsm_state == State.ARMING:
if self.armed and self.relative_alt > 19:
self.gcs_status = DroneInfo.Run
response = self.set_mode(flight_modes.MISSION)
if response.success:
self.fsm_state = State.SET_MISSION
else:
if self.cmd_try_again:
response = self.arm()
if response.success:
self.cmd_try_again = False
# ------------------------------------------------------------------------------ #
elif self.fsm_state == State.SYNC_WP_IDX:
if self.manual_mission.mission_idx == self.active_mission_idx:
response = self.set_mode(flight_modes.MISSION)
if response.success:
self.fsm_state = State.SET_MISSION
else:
print("[drone]: Manual idx = {}, Active idx = {}".format(
self.manual_mission.mission_idx, self.active_mission_idx))
self.set_current_mission_pub.publish(
Int16(self.manual_mission.mission_idx))
# ------------------------------------------------------------------------------ #
elif self.fsm_state == State.SET_MISSION:
if self.sub_mode == "Mission":
self.gcs_status = DroneInfo.Run
request = ChangeSpeedRequest(MISSION_SPEED)
response = self.change_speed(request)
if response.success:
self.fsm_state = State.SET_SPEED
else:
if self.cmd_try_again:
response = self.set_mode(flight_modes.MISSION)
if response.success:
self.cmd_try_again = False
# ------------------------------------------------------------------------------ #
elif self.fsm_state == State.SET_SPEED:
if self.speed_ack:
self.fsm_state = State.FLYING_MISSION
self.speed_ack = False
else:
if self.cmd_try_again:
request = ChangeSpeedRequest(MISSION_SPEED)
response = self.change_speed(request)
if response.success:
self.cmd_try_again = False
# ------------------------------------------------------------------------------ #
elif self.fsm_state == State.FLYING_MISSION:
if self.new_mission:
self.manual_mission.start_running()
self.active_mission_idx = 0
self.new_mission = False
# elif self.manual_mission.fsm_state == manual_mission.State.ON_THE_WAY:
self.fsm_state = State.CLEARING_MISSION
self.clear_mission_pub.publish(Empty())
if self.is_on_goal():
# self.fsm_state = State.LANDING
lat_goal = self.active_mission_gps[-1].latitude
lon_goal = self.active_mission_gps[-1].longitude
# reposition to a lower altitude above the goal
request = GotoWaypointRequest(relative_alt=True,
ground_speed=MISSION_SPEED,
latitude=lat_goal,
longitude=lon_goal,
altitude=10)
response = self.reposition(request)
if response.success:
rospy.Timer(rospy.Duration(HOLD_TIME),
self.hold_cb,
oneshot=True)
self.hold = True
self.fsm_state = State.HOLD
elif self.loiter and self.ground_speed < 0.5:
self.gcs_status = DroneInfo.holding
# ------------------------------------------------------------------------------ #
elif self.fsm_state == State.REPOSITION:
if self.new_mission:
self.manual_mission.start_running()
self.active_mission_idx = 0
self.new_mission = False
# elif self.manual_mission.fsm_state == manual_mission.State.ON_THE_WAY:
self.fsm_state = State.CLEARING_MISSION
self.clear_mission_pub.publish(Empty())
else:
if self.cmd_try_again:
request = GotoWaypointRequest(
relative_alt=True,
ground_speed=MISSION_SPEED,
latitude=self.holding_waypoint.position.latitude,
longitude=self.holding_waypoint.position.longitude,
altitude=self.holding_waypoint.position.altitude)
response = self.reposition(request)
if response.success:
self.cmd_try_again = False
# ------------------------------------------------------------------------------ #
elif self.fsm_state == State.LANDING:
if self.state == "Standby":
self.fsm_state = State.GROUNDED
self.gcs_status = DroneInfo.Land
# reset states
self.reset()
self.manual_mission.reset()
else:
if self.cmd_try_again:
response = self.set_mode(flight_modes.MISSION)
if response.success:
self.cmd_try_again = False
# ------------------------------------------------------------------------------ #
elif self.fsm_state == State.CLEARING_MISSION:
if self.upload_done:
self.upload_done = False
self.fsm_state = State.REQUESTING_UPLOAD
# ------------------------------------------------------------------------------ #
elif self.fsm_state == State.PAUSED:
if self.state == "Active" and self.sub_mode == "Mission":
self.fsm_state = State.FLYING_MISSION
elif self.state == "Standby" or not self.armed:
self.fsm_state = State.GROUNDED
# ------------------------------------------------------------------------------ #
elif self.fsm_state == State.WAITING:
if not self.wait:
self.fsm_state = State.REQUESTING_UPLOAD
# ------------------------------------------------------------------------------ #
elif self.fsm_state == State.HOLD:
if not self.hold:
response = self.set_mode(flight_modes.MISSION)
if response.success:
self.fsm_state = State.LANDING
else:
if self.cmd_try_again:
lat_goal = self.active_mission_gps[-1].latitude
lon_goal = self.active_mission_gps[-1].longitude
# reposition to a lower altitude above the goal
request = GotoWaypointRequest(relative_alt=True,
ground_speed=MISSION_SPEED,
latitude=lat_goal,
longitude=lon_goal,
altitude=10)
response = self.reposition(request)
if response.success:
self.cmd_try_again = False
# prepare to call AuctionService in the node that will be the auctioneer
service_path = nearest_node + '/auction_server'
rospy.wait_for_service(service_path)
auctioneer_service = rospy.ServiceProxy(service_path,
auction_srvs.srv.AuctionService)
try:
auctioneer_service_resp = auctioneer_service(role, auction_type,
sending_node,
nodes_collected,
auction_data)
rospy.loginfo("[message] Auctioneer response")
rospy.loginfo(auctioneer_service_resp)
except rospy.ServiceException, e:
rospy.logwarn("Service did not process request: %s", e)
# here we want to unregister services (clean-up procedure)
# ATTENTION: here we need to revert the order of calling services, since the call to auction_config_server will shutdown auctioneer services
rospy.set_param('/auction_closed', True)
# Getting results for statistic studies
# get collected nodes
nodes_collected_list = rospy.get_param('/nodes_collected').split(',')
# remove '' strings
while '' in nodes_collected_list:
nodes_collected_list.remove('')
# remove duplicates
nodes_collected_list = list(set(nodes_collected_list))
nodes_collected_str = ','.join(nodes_collected_list)
def new_mission_cb(mission_msg):
try:
auth = rospy.ServiceProxy('/dji_sdk/sdk_control_authority', SDKControlAuthority)
rospy.loginfo('Service. Sdk control authority:')
resp = auth(1)
rospy.loginfo(resp.result)
if not resp.result:
return
except rospy.ServiceException as e:
rospy.logwarn(e)
return
try:
mission = rospy.ServiceProxy('/dji_sdk/mission_waypoint_upload', MissionWpUpload)
mission_task_msg = MissionWaypointTask()
mission_task_msg.velocity_range = 10;
mission_task_msg.idle_velocity = 10;
mission_task_msg.action_on_finish = MissionWaypointTask.FINISH_RETURN_TO_HOME;
mission_task_msg.mission_exec_times = 1;
mission_task_msg.yaw_mode = MissionWaypointTask.YAW_MODE_AUTO;
mission_task_msg.trace_mode = MissionWaypointTask.TRACE_POINT;
mission_task_msg.action_on_rc_lost = MissionWaypointTask.ACTION_AUTO;
mission_task_msg.gimbal_pitch_mode = MissionWaypointTask.GIMBAL_PITCH_FREE;
rospy.loginfo('Received mission from objective:')
rospy.loginfo(mission_msg)
for item in mission_msg.waypoints:
cmd_parameter = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
mission_task_msg.mission_waypoint.append(MissionWaypoint(
latitude = item.latitude,
longitude = item.longitude,
altitude = item.altitude,
damping_distance = 2,
target_yaw = 0,
has_action = 1,
target_gimbal_pitch = 0,
turn_mode = 0,
action_time_limit = 64000,
waypoint_action = MissionWaypointAction(
action_repeat = 10,
command_parameter = cmd_parameter )))
rospy.logdebug(mission_task_msg)
rospy.loginfo('Service. Mission waypoint upload:')
resp = mission(mission_task_msg)
rospy.loginfo(resp.result)
if not resp.result:
return
except rospy.ServiceException as e:
rospy.logwarn(e)
return
try:
start = rospy.ServiceProxy('/dji_sdk/mission_waypoint_action', MissionWpAction)
resp = start(0)
rospy.loginfo('Service. Mission waypoint action:')
rospy.loginfo(resp.result)
if not resp.result:
return
except rospy.ServiceException as e:
rospy.logwarn(e)
return
rospy.logdebug("############### START EPISODE=>" + str(x))
cumulated_reward = 0
done = False
if qlearn.epsilon > 0.05:
qlearn.epsilon *= epsilon_discount
# Initialize the environment and get first state of the robot
observation = env.reset()
state = ''.join(map(str, observation))
# Show on screen the actual situation of the robot
# env.render()
# for each episode, we test the robot for nsteps
for i in range(nsteps):
rospy.logwarn("############### Start Step=>" + str(i))
# Pick an action based on the current state
action = qlearn.chooseAction(state)
rospy.logdebug("Next action is:%d", action)
# Execute the action in the environment and get feedback
observation, reward, done, info = env.step(action)
rospy.logdebug(str(observation) + " " + str(reward))
cumulated_reward += reward
if highest_reward < cumulated_reward:
highest_reward = cumulated_reward
nextState = ''.join(map(str, observation))
# Make the algorithm learn based on the results
rospy.logdebug("# state we were=>" + str(state))
def __init__(self, language_code='it-IT', last_contexts=None):
"""Initialize all params and load data"""
""" Constants and params """
self.CHUNK = 4096
self.FORMAT = pyaudio.paInt16
self.CHANNELS = 1
self.RATE = 44100 #was 16000
self.USE_AUDIO_SERVER = rospy.get_param(
'/dialogflow_client/use_audio_server', False)
if self.USE_AUDIO_SERVER:
print("USO AUDIOSERVER")
else:
print("USO IL MICROFONO")
self.PLAY_AUDIO = rospy.get_param('/dialogflow_client/play_audio',
True)
self.DEBUG = rospy.get_param('/dialogflow_client/debug', False)
# Register Ctrl-C sigint
signal.signal(signal.SIGINT, self._signal_handler)
""" Dialogflow setup """
# Get hints/clues
rp = rospkg.RosPack()
file_dir = rp.get_path('dialogflow_ros') + '/config/context.yaml'
with open(file_dir, 'r') as f:
try:
self.phrase_hints = load(f)
except YAMLError:
rospy.logwarn(
"DF_CLIENT: Unable to open phrase hints yaml file!")
self.phrase_hints = []
# Dialogflow params
project_id = rospy.get_param('/dialogflow_client/project_id',
'robot-15de1')
session_id = str(uuid4()) # Random
self._language_code = language_code
self.last_contexts = last_contexts if last_contexts else []
# DF Audio Setup
audio_encoding = AudioEncoding.AUDIO_ENCODING_LINEAR_16
# Possibel models: video, phone_call, command_and_search, default
self._audio_config = InputAudioConfig(
audio_encoding=audio_encoding,
language_code=self._language_code,
sample_rate_hertz=self.RATE,
phrase_hints=self.phrase_hints,
model='default')
self._output_audio_config = OutputAudioConfig(
audio_encoding=OutputAudioEncoding.OUTPUT_AUDIO_ENCODING_LINEAR_16)
# Create a session
self._session_cli = dialogflow_v2beta1.SessionsClient()
self._session = self._session_cli.session_path(project_id, session_id)
rospy.logdebug("DF_CLIENT: Session Path: {}".format(self._session))
""" ROS Setup """
results_topic = rospy.get_param('/dialogflow_client/results_topic',
'/dialogflow_client/results')
requests_topic = rospy.get_param('/dialogflow_client/requests_topic',
'/dialogflow_client/requests')
text_req_topic = requests_topic + '/string_msg'
text_event_topic = requests_topic + '/string_event'
msg_req_topic = requests_topic + '/df_msg'
event_req_topic = requests_topic + '/df_event'
self._results_pub = rospy.Publisher(results_topic,
DialogflowResult,
queue_size=10)
rospy.Subscriber(text_req_topic, String, self._text_request_cb)
rospy.Subscriber(text_event_topic, String, self._text_event_cb)
rospy.Subscriber(msg_req_topic, DialogflowRequest,
self._msg_request_cb)
rospy.Subscriber(event_req_topic, DialogflowEvent,
self._event_request_cb)
self.pub_chatter = rospy.Publisher('/chatter', String, queue_size=10)
""" Audio setup """
# Mic stream input setup
self.audio = pyaudio.PyAudio()
self._server_name = rospy.get_param('/dialogflow_client/server_name',
'127.0.0.1')
self._port = rospy.get_param('/dialogflow_client/port', 4444)
print("Audio Server at: ", self._server_name, ", on port ", self._port)
if self.PLAY_AUDIO:
self._create_audio_output()
rospy.logdebug("DF_CLIENT: Last Contexts: {}".format(
self.last_contexts))
rospy.loginfo("DF_CLIENT: Ready!")
def _gen_xarco():
rospy.init_node('modify_xacro')
if len(sys.argv) < 3:
rospy.logwarn('usage: modify_xacro_node model new_model replace')
return
model = sys.argv[1]
new_model = sys.argv[2]
replace = False
if len(sys.argv) == 4:
if sys.argv[3].upper() == 'REPLACE':
replace = True
rospy.logwarn('origin xacro file will be replaced')
rospy.wait_for_service('tmr/ask_item')
ask_item = rospy.ServiceProxy('tmr/ask_item', AskItem)
res_dh = ask_item('dh', 'DHTable', 1.0)
res_dd = ask_item('dd', 'DeltaDH', 1.0)
if not res_dh.value.startswith('DHTable={') or not res_dh.value.endswith('}'):
rospy.logerr('invalid dh')
return
if not res_dd.value.startswith('DeltaDH={') or not res_dd.value.endswith('}'):
rospy.logerr('invalid delta_dh')
return
rospy.loginfo(res_dh.value)
rospy.loginfo(res_dd.value)
dh_strs = res_dh.value[9:-1].split(',')
dd_strs = res_dd.value[9:-1].split(',')
'''
res_dh = 'DHTable={0,-90,0,145.1,0,-277,277,-90,0,429,0,0,-187,187,0,0,411.5,0,0,-162,162,90,90,0,-122.2,0,-187,187,0,90,0,106,0,-187,187,0,0,0,114.4,0,-277,277}'
res_dd = 'DeltaDH={-0.001059821,0.02508766,0.009534874,0,0.001116668,0.06614932,0.308224,0.0287381,0.06797475,-0.0319523,0.3752921,0.06614756,-0.006998898,0.06792655,-0.06083903,0.02092069,0.02965812,-0.1331249,0.06793034,0.02077797,0.08265772,0.03200645,0.01835932,0.06145732,0.08273286,0.6686108,0.6972408,-0.1793097,-0.0794057,1.425708}'
rospy.loginfo(res_dh)
rospy.loginfo(res_dd)
dh_strs = res_dh[9:-1].split(',')
dd_strs = res_dd[9:-1].split(',')
'''
if len(dh_strs) != 42:
rospy.logerr('invalid dh')
return
if len(dd_strs) != 30:
rospy.logerr('invalid delta_dh')
return
dh = [float(i) for i in dh_strs]
dd = [float(i) for i in dd_strs]
# find xacro path
curr_path = os.path.dirname(os.path.abspath(__file__))
dirs = ['src', 'devel']
ind = -1
for d in dirs:
ind = curr_path.find(d)
if (ind != -1):
break
if (ind == -1) :
rospy.logerr('can not find workspace directory')
return
src_path = curr_path[:ind] + 'src'
xacro_path = ''
for dirpath, dirnames, filenames in os.walk(src_path):
if dirpath.endswith('tmr_description'):
xacro_path = dirpath + '/xacro'
break
if (xacro_path == ''):
rospy.logerr('can not find xacro directory')
return
xacro_name = '/macro.' + model + '.urdf.xacro'
new_xacro_name = '/macro.' + new_model + '.urdf.xacro'
file_in = xacro_path + xacro_name
file_out = xacro_path + new_xacro_name
link_tag = '<!--LinkDescription-->'
link_head = '<?xml version=\'1.0\' encoding=\'UTF-8\'?>\n'
link_start = '<data xmlns:xacro="http://wiki.ros.org/xacro">'
link_end = '</data>'
rospy.loginfo(file_in)
fr = open(file_in, 'r')
data_in = fr.read()
fr.close()
datas = data_in.split(link_tag)
if len(datas) < 3:
rospy.logerr('invalid tmr...xacro')
return
link_data = link_start + datas[1] + link_end
root = ET.fromstring(link_data)
udh = urdf_DH_from_tm_DH(dh, dd)
xyzs, rpys = xyzrpys_from_urdf_DH(udh)
modify_urdf(root, xyzs, rpys, udh, '${prefix}')
link_data = ET.tostring(root, encoding='UTF-8').decode('UTF-8')
link_data = link_data.replace('ns0', 'xacro')
link_data = link_data.replace(link_head, '', 1)
link_data = link_data.replace(link_start, link_tag, 1)
link_data = link_data.replace(link_end, link_tag, 1)
data_out = datas[0] + link_data + datas[2]
file_save = ''
if replace:
file_save = file_in
rospy.loginfo('copy and rename origin xacro file')
shutil.copyfile(file_in, file_out)
else:
file_save = file_out
rospy.loginfo(file_save)
fw = open(file_save, 'w')
fw.write(data_out)
fw.close()
def get_classification(self, image):
"""Determines the color of the traffic light in the image
Args:
image (cv::Mat): image containing the traffic light
Returns:
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
#TODO implement light color prediction
with self.detection_graph.as_default():
with tf.Session(graph=self.detection_graph) as sess:
image_tensor = self.detection_graph.get_tensor_by_name('image_tensor:0')
detect_boxes = self.detection_graph.get_tensor_by_name('detection_boxes:0')
detect_scores = self.detection_graph.get_tensor_by_name('detection_scores:0')
detect_classes = self.detection_graph.get_tensor_by_name('detection_classes:0')
num_detections = self.detection_graph.get_tensor_by_name('num_detections:0')
image_expanded = np.expand_dims(image, axis=0)
(boxes, scores, classes, num) = sess.run(
[detect_boxes, detect_scores, detect_classes, num_detections],
feed_dict={image_tensor: image_expanded})
vis_util.visualize_boxes_and_labels_on_image_array(
image,
np.squeeze(boxes),
np.squeeze(classes).astype(np.int32),
np.squeeze(scores),
self.category_index,
use_normalized_coordinates=True,
max_boxes_to_draw=5,
line_thickness=5)
boxes = np.squeeze(boxes)
scores = np.squeeze(scores)
classes = np.squeeze(classes).astype(np.int32)
keep = scores > 0.5
# if scores[0] > 0.5:
# state = self.class_map[classes[0]]
# else:
# state = TrafficLight.UNKNOWN
#### select the dominant traffic light class
if np.any(keep):
boxes = boxes[keep]
scores = scores[keep]
classes = classes[keep]
members, index, counts = np.unique(classes, return_inverse=True, return_counts=True)
member_scores = np.zeros((len(members),))
for i in range(len(members)):
member_scores[i] = np.sum(scores[index == i])
select = np.argmax(member_scores)
winner = members[select]
state = self.class_map[winner]
else:
state = TrafficLight.UNKNOWN
### log
rospy.logwarn("detected light : {} ".format(classes[0]))
return image, state
def close(self):
try:
self._srv_arm.unregister()
except AttributeError, e:
rospy.logwarn("Failed to unregister /tuck_arm service")
self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
self.server.start()
rospy.spin()
def actionCb(self, goal):
""" Take an input command of width to open gripper. """
rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
# send command to gripper
self.model.setCommand(goal.command)
# publish feedback
while True:
if self.server.is_preempt_requested():
self.server.set_preemtped()
rospy.loginfo('Gripper Controller: Preempted.')
return
# TODO: get joint position, break when we have reached goal
break
self.server.set_succeeded()
rospy.loginfo('Gripper Controller: Succeeded.')
def stateCb(self, msg):
self.state = msg
if __name__=='__main__':
try:
rospy.logwarn("Please use gripper_controller (no .py)")
GripperActionController()
except rospy.ROSInterruptException:
rospy.loginfo('Hasta la Vista...')
def _signal_handler(self, signal, frame):
rospy.logwarn("\nDF_CLIENT: SIGINT caught!")
self.exit()
class CalculatePersonPosition(smach.State):
def __init__(self,
controller,
controller_2=None,
sensor=None,
max_distance=2.5,
onlyhorizontal=False,
knownperson=True):
self.person_sensor = controller
self.max_distance = max_distance
self.person_id = sensor
self.talk_known = controller_2
self.ignoreknownperson = knownperson
self.talks = [
'Oh, ich denke Dich habe ich schon begrüsst',
'Dich kenne ich schon, ich mache weiter',
'Oh schön dich wieder zu sehen, bis gleich.',
'Wen haben wir denn da, dich kenne ich. Gleich gehen die Vorträge los!',
'Hallo noch mal. Wie wäre es wenn wir uns nachher unterhalten?'
]
self.counter = 0
# https://answers.ros.org/question/10777/service-exception-using-tf-listener-in-rospy
self.tf = tf.TransformListener()
input = []
self.onlyhorizontal = onlyhorizontal
if onlyhorizontal:
input = ['old_vertical']
smach.State.__init__(
self,
input_keys=input,
outcomes=['success', 'repeat', 'no_person_found', 'known'],
output_keys=['person_angle_vertical', 'person_angle_horizontal'])
def execute(self, userdata):
self.person = None
self.person_sensor.clearPerson()
rospy.sleep(0.1)
self.person = self.person_sensor.getDetectedPerson()
self.pose = None
self.transformid = None
self.dist = self.max_distance
for p in self.person:
pose = p.pose
dist = distance(pose.pose.position)
rospy.logwarn
if not self.tf.frameExists('base_link'):
rospy.logwarn('Base_link does not exist!')
if not self.tf.frameExists(pose.header.frame_id):
rospy.logwarn('%s does not exist!' % str(pose.header.frame_id))
if dist < self.dist and self.tf.frameExists(
pose.header.frame_id) and self.tf.frameExists('base_link'):
try:
print(p.pose)
self.tf.waitForTransform('base_link',
'CameraDepth_optical_frame',
rospy.Time.now(),
rospy.Duration(4.0))
po = self.tf.transformPose("base_link", pose)
self.dist = dist
self.pose = po.pose
self.transformid = p.transformid
except Exception, e:
print("Exception %s" % e)
return 'repeat'
if dist > self.max_distance and self.max_distance == self.dist:
print('Detected person to far away. Distance: %s ' % dist)
if self.pose:
(vertical, horizontal) = rotation(self.pose)
self.counter = 0
if self.onlyhorizontal:
userdata.person_angle_vertical = userdata.old_vertical
else:
userdata.person_angle_vertical = vertical
userdata.person_angle_horizontal = horizontal
if self.dist < 1.8 and self.transformid is not None:
try:
known, name = self.person_id.identify(self.transformid)
if known and self.ignoreknownperson:
rospy.loginfo("Person is known, iterating")
if self.talk_known is not None:
self.talk_known.say_something(
random.choice(self.talks))
return 'known'
else:
return 'success'
except Exception, e:
rospy.logwarn(
"Something went wront while identifying a person")
return 'success'
def process_trajectory(self, traj):
num_points = len(traj.points)
# make sure the joints in the goal match the joints of the controller
if set(self.joint_names) != set(traj.joint_names):
res = FollowJointTrajectoryResult().INVALID_JOINTS
msg = 'Incoming trajectory joints do not match the joints of the controller'
rospy.logerr(msg)
rospy.logerr(' self.joint_names={}' % (set(self.joint_names)))
rospy.logerr(' traj.joint_names={}' % (set(traj.joint_names)))
self.action_server.set_aborted(result=res, text=msg)
return
# make sure trajectory is not empty
if num_points == 0:
msg = 'Incoming trajectory is empty'
rospy.logerr(msg)
self.action_server.set_aborted(text=msg)
return
# correlate the joints we're commanding to the joints in the message
# map from an index of joint in the controller to an index in the trajectory
lookup = [traj.joint_names.index(joint) for joint in self.joint_names]
durations = [0.0] * num_points
# find out the duration of each segment in the trajectory
durations[0] = traj.points[0].time_from_start.to_sec()
for i in range(1, num_points):
durations[i] = (traj.points[i].time_from_start -
traj.points[i - 1].time_from_start).to_sec()
if not traj.points[0].positions:
res = FollowJointTrajectoryResult().INVALID_GOAL
msg = 'First point of trajectory has no positions'
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
trajectory = []
time = rospy.Time.now() + rospy.Duration(0.01)
for i in range(num_points):
seg = Segment(self.num_joints)
if traj.header.stamp == rospy.Time(0.0):
seg.start_time = (time + traj.points[i].time_from_start
).to_sec() - durations[i]
else:
seg.start_time = (
traj.header.stamp +
traj.points[i].time_from_start).to_sec() - durations[i]
seg.duration = durations[i]
# Checks that the incoming segment has the right number of elements.
if traj.points[i].velocities and len(
traj.points[i].velocities) != self.num_joints:
res = FollowJointTrajectoryResult().INVALID_GOAL
msg = 'Command point %d has %d elements for the velocities' % (
i, len(traj.points[i].velocities))
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
if len(traj.points[i].positions) != self.num_joints:
res = FollowJointTrajectoryResult().INVALID_GOAL
msg = 'Command point %d has %d elements for the positions' % (
i, len(traj.points[i].positions))
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
for j in range(self.num_joints):
if traj.points[i].velocities:
seg.velocities[j] = traj.points[i].velocities[lookup[j]]
if traj.points[i].positions:
seg.positions[j] = traj.points[i].positions[lookup[j]]
trajectory.append(seg)
rospy.loginfo('Trajectory start requested at %.3lf, waiting...',
traj.header.stamp.to_sec())
rate = rospy.Rate(self.update_rate)
while traj.header.stamp > time:
time = rospy.Time.now()
rate.sleep()
end_time = traj.header.stamp + rospy.Duration(sum(durations))
seg_end_times = [
rospy.Time.from_sec(trajectory[seg].start_time + durations[seg])
for seg in range(len(trajectory))
]
rospy.loginfo(
'Trajectory start time is %.3lf, end time is %.3lf, total duration is %.3lf',
time.to_sec(), end_time.to_sec(), sum(durations))
self.trajectory = trajectory
traj_start_time = rospy.Time.now()
for seg in range(len(trajectory)):
rospy.logdebug(
'current segment is %d time left %f cur time %f' %
(seg, durations[seg] -
(time.to_sec() - trajectory[seg].start_time), time.to_sec()))
rospy.logdebug('goal positions are: %s' %
str(trajectory[seg].positions))
# first point in trajectories calculated by OMPL is current position with duration of 0 seconds, skip it
if durations[seg] == 0:
rospy.logdebug(
'skipping segment %d with duration of 0 seconds' % seg)
continue
multi_packet = {}
for port, joints in self.port_to_joints.items():
vals = []
for joint in joints:
j = self.joint_names.index(joint)
start_position = self.joint_states[joint].current_pos
if seg != 0:
start_position = trajectory[seg - 1].positions[j]
desired_position = trajectory[seg].positions[j]
desired_velocity = max(
self.min_velocity,
abs(desired_position - start_position) /
durations[seg])
self.msg.desired.positions[j] = desired_position
self.msg.desired.velocities[j] = desired_velocity
# probably need a more elegant way of figuring out if we are dealing with a dual controller
if hasattr(self.joint_to_controller[joint], "master_id"):
master_id = self.joint_to_controller[joint].master_id
slave_id = self.joint_to_controller[joint].slave_id
master_pos, slave_pos = self.joint_to_controller[
joint].pos_rad_to_raw(desired_position)
spd = self.joint_to_controller[joint].spd_rad_to_raw(
desired_velocity)
vals.append((master_id, master_pos, spd))
vals.append((slave_id, slave_pos, spd))
else:
motor_id = self.joint_to_controller[joint].motor_id
pos = self.joint_to_controller[joint].pos_rad_to_raw(
desired_position)
spd = self.joint_to_controller[joint].spd_rad_to_raw(
desired_velocity)
vals.append((motor_id, pos, spd))
multi_packet[port] = vals
for port, vals in multi_packet.items():
self.port_to_io[port].set_multi_position_and_speed(vals)
while time < seg_end_times[seg]:
# check if new trajectory was received, if so abort current trajectory execution
# by setting the goal to the current position
if self.action_server.is_preempt_requested():
msg = 'New trajectory received. Aborting old trajectory.'
multi_packet = {}
for port, joints in self.port_to_joints.items():
vals = []
for joint in joints:
cur_pos = self.joint_states[joint].current_pos
motor_id = self.joint_to_controller[joint].motor_id
pos = self.joint_to_controller[
joint].pos_rad_to_raw(cur_pos)
vals.append((motor_id, pos))
multi_packet[port] = vals
for port, vals in multi_packet.items():
self.port_to_io[port].set_multi_position(vals)
self.action_server.set_preempted(text=msg)
rospy.logwarn(msg)
return
rate.sleep()
time = rospy.Time.now()
# Verifies trajectory constraints
for j, joint in enumerate(self.joint_names):
if self.trajectory_constraints[
j] > 0 and self.msg.error.positions[
j] > self.trajectory_constraints[j]:
res = FollowJointTrajectoryResult().PATH_TOLERANCE_VIOLATED
msg = 'Unsatisfied position constraint for %s, trajectory point %d, %f is larger than %f' % \
(joint, seg, self.msg.error.positions[j], self.trajectory_constraints[j])
rospy.logwarn(msg)
self.action_server.set_aborted(result=res, text=msg)
return
# let motors roll for specified amount of time
rospy.sleep(self.goal_time_constraint)
for i, j in enumerate(self.joint_names):
rospy.logdebug(
'desired pos was %f, actual pos is %f, error is %f' %
(trajectory[-1].positions[i], self.joint_states[j].current_pos,
self.joint_states[j].current_pos -
trajectory[-1].positions[i]))
# Checks that we have ended inside the goal constraints
for (joint, pos_error, pos_constraint) in zip(self.joint_names,
self.msg.error.positions,
self.goal_constraints):
if pos_constraint > 0 and abs(pos_error) > pos_constraint:
res = FollowJointTrajectoryResult().GOAL_TOLERANCE_VIOLATED
msg = 'Aborting because %s joint wound up outside the goal constraints, %f is larger than %f' % \
(joint, pos_error, pos_constraint)
rospy.logwarn(msg)
self.action_server.set_aborted(result=res, text=msg)
break
else:
res = FollowJointTrajectoryResult().SUCCESSFUL
msg = 'Trajectory execution successfully completed'
rospy.loginfo(msg)
self.action_server.set_succeeded(result=res, text=msg)
def RTL(self):
init_time = time.time()
velocity = 0.3
ds = velocity / 60.0
transition_time = time.time() - init_time
rospy.logwarn('Time in setup: ' + str(transition_time))
self.rate.sleep()
height = self.drone_pose.pose.position.z
rospy.loginfo('Position: (' + str(self.drone_pose.pose.position.x) +
', ' + str(self.drone_pose.pose.position.y) + ', ' +
str(self.drone_pose.pose.position.z) + ')')
self.set_position(0, 0, height)
self.rate.sleep()
rospy.loginfo('Position: (' + str(self.drone_pose.pose.position.x) +
', ' + str(self.drone_pose.pose.position.y) + ', ' +
str(self.drone_pose.pose.position.z) + ')')
rospy.loginfo('Goal Position: (' +
str(self.goal_pose.pose.position.x) + ', ' +
str(self.goal_pose.pose.position.y) + ', ' +
str(self.goal_pose.pose.position.z) + ')')
while not self.chegou():
rospy.loginfo('Executing State RTL')
rospy.loginfo("STARING HOME")
self.set_position(0, 0, height)
self.rate.sleep()
t = 0
self.set_position(0, 0, height - ds)
self.rate.sleep()
init_time = time.time()
while not (self.drone_pose.pose.position.z < -0.1) and time.time(
) - init_time < (height / velocity) * 1.2: #20% tolerance in time
rospy.loginfo('Executing State RTL')
rospy.loginfo('Height: ' +
str(abs(self.drone_pose.pose.position.z)))
################# Velocity Control #################
self.set_vel(0, 0, -0.3, 0, 0, 0)
#################### Position Control ##############
# if not self.chegou():
# rospy.logwarn ('LANDING AT ' + str(velocity) + 'm/s')
# if t <= height:
# t += ds
# self.set_position(0,0,height - t)
# self.rate.sleep()
#
# else:
# if t <= height:
# t += ds
# self.set_position(0,0,height - t)
# else:
# self.set_position(0,0,0)
# self.rate.sleep()
####################################################
rospy.logwarn("DESARMANDO DRONE " + self.mav_name)
self.arm(False)
return "succeeded"
def traffic_cb(self, msg):
# Callback for /traffic_waypoint message.
# Set speed for waypoints before red lights and stop points.
# Recover waypoint stop position
stop_waypoint_index = msg.data
# Check if its a new index point
if self.last_stop_index == stop_waypoint_index:
return
should_brake = True
if self.current_waypoint_index is None:
# Ignore
return
if stop_waypoint_index == -1:
# No Red light ahead, recover original waypoitns
# Accelerate
stop_waypoint_index = self.current_waypoint_index + LOOKAHEAD_WPS
if stop_waypoint_index > len(self.waypoints):
stop_waypoint_index = len(self.waypoints)
should_brake = False
distance_full = 99
rospy.logwarn("Accelerate Car")
else:
# Compute distance to stop
distance_full = self.distance(self.waypoints,
self.current_waypoint_index,
stop_waypoint_index - 1)
if distance_full > 100:
#ignore
rospy.logwarn("Ignore Light Distance")
return
# Set the number of slow (or accelerate) steps
steps = float(int(distance_full / 10))
if steps == 0:
steps = 1
if distance_full < 30:
# Minimum number of steps
steps = 4
self.last_stop_index = stop_waypoint_index
# Compute linear acceleration/slowdown
current_speed = self.get_waypoint_velocity(
self.waypoints[self.current_waypoint_index])
if not should_brake and current_speed >= MAX_SPEED:
# Ignore, no red lights and speed is already high
rospy.logwarn("Ignore Light Speed")
return
# Step slow down or acceleration
linear_acceleration = current_speed / steps
if not should_brake:
linear_acceleration = MAX_SPEED / steps
distance_divider = (distance_full / steps)
# Update waypoints
for i in range(self.current_waypoint_index, stop_waypoint_index + 1):
# Compute partial speed until stop
velocity = 0
short_distance = self.distance(self.waypoints,
self.current_waypoint_index, i)
multi = int(short_distance / distance_divider)
diff_velocity = float(multi + 1) * linear_acceleration
if should_brake:
# Braking
velocity = current_speed - diff_velocity
if velocity < 0:
velocity = 0
else:
# Accelerating
velocity = current_speed + diff_velocity
if velocity > MAX_SPEED:
velocity = MAX_SPEED
rospy.logwarn("Set speed {} {} {} {} {}".format(
i, velocity, multi, linear_acceleration, short_distance))
self.set_waypoint_velocity(self.waypoints, i, velocity)
# Force to Publish new waypoints
self.waypoints_changed = True
def __init__(self):
rospy.init_node('tl_detector')
self.i = 0
#stop line
self.traffic_line_index = []
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
#traffic light
self.lights = []
self.traffic_light_index = []
self.signal_classes = ['STOP', 'STOP', 'GO', 'Unknown', 'Unknown']
sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
#position
self.position = None
sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
#waypoints
self.waypoints = None
sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
#camera_image
self.camera_image = None
self.has_image = False
sub6 = rospy.Subscriber('/image_color', Image, self.image_cb)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.car_position = rospy.Publisher('/car_position',
Int32,
queue_size=1)
'''
/vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and
helps you acquire an accurate ground truth data source for the traffic light
classifier by sending the current color state of all traffic lights in the
simulator. When testing on the vehicle, the color state will not be available. You'll need to
rely on the position of the light and the camera image to predict it.
'''
self.bridge = CvBridge()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.i = 0
#load classifier,time comsuming
self.light_classifier = TLClassifier()
self.color_Classifier = ColorClassifier()
rospy.logwarn('You can launch simulator now!')
#set spin rate according to time required for classification
rate = rospy.Rate(5)
start_time = 0
while not start_time:
start_time = rospy.Time.now().to_sec()
while not rospy.is_shutdown():
self.detect_tl()
rate.sleep()
def execute(self):
if self.needs_reset:
rospy.logwarn('Success Decorator needs already returned '+ self.get_status())
return self.get_status()
if not self.finished:
if not self.started:
if self.timeout == -1: # dont start timer, wait forever
self.started = True
else:
self.timer = rospy.Timer(rospy.Duration(1.0*self.timeout), self.timed_out, oneshot=True)
self.started = True
self.child_status_ = self.children_[0].execute()
if self.child_status_[:7] == 'SUCCESS':
rospy.logwarn('WAIT SUCCESS DECORATOR ['+self.name_+']: REPORTED SUCCESS')
self.needs_reset = True
return self.set_status('SUCCESS')
elif self.child_status_ == 'RUNNING':
rospy.logwarn('WAIT SUCCESS DECORATOR ['+self.name_+']: CHILD RUNNING, WAITING')
return self.set_status('RUNNING')
else:
rospy.logwarn('WAIT SUCCESS DECORATOR ['+self.name_+']: CHILD FAILED, RESET, WAITING')
self.children_[0].reset()
return self.set_status('RUNNING')
else: # started
self.child_status_ = self.children_[0].execute()
if self.child_status_[:7] == 'SUCCESS':
rospy.logwarn('WAIT SUCCESS DECORATOR ['+self.name_+']: REPORTED SUCCESS')
self.needs_reset = True
return self.set_status('SUCCESS')
elif self.child_status_ == 'RUNNING':
rospy.logwarn('WAIT SUCCESS DECORATOR ['+self.name_+']: CHILD RUNNING, WAITING')
return self.set_status('RUNNING')
else:
rospy.logwarn('WAIT SUCCESS DECORATOR ['+self.name_+']: CHILD FAILED, RESET, WAITING')
self.children_[0].reset()
return self.set_status('RUNNING')
else: # finished
rospy.logwarn('Success Decorator timed out')
return self.set_status('FAILURE')
def __init__(self, default_port='/dev/ttyUSB1'):
"""
@param default_port: default serial port to use for
establishing a connection to the UM7 IMU sensor.
This will be overridden by ~port param if
available.
"""
rospy.init_node('imu_um7')
self.port = rospy.get_param('~port', default_port)
self.frame_id = rospy.get_param('~frame_id', "/imu")
self.gps_frame_id = rospy.get_param('~gps_frame_id', "/gps")
self.throttle_rate = rospy.get_param('~throttle_rate', 10000)
self.reset_mag = rospy.get_param('~reset_mag', False)
self.reset_accel = rospy.get_param('~reset_accel', False)
self.mag_zero_x = rospy.get_param('~mag_zero_x', False)
self.mag_zero_y = rospy.get_param('~mag_zero_y', False)
self.mag_zero_z = rospy.get_param('~mag_zero_z', False)
rospy.loginfo("serial port: %s" % (self.port))
self.link = rospy.get_param('~link', 'imu_link')
rospy.loginfo("tf link: {}".format(self.link))
self.imu_data = Imu()
self.imu_data = Imu(header=rospy.Header(frame_id=self.link))
# These covariance calculations are based on those bound in the existing
# UM7 ROS package at:
# https://github.com/ros-drivers/um7
linear_acceleration_stdev = rospy.get_param(
"~linear_acceleration_stdev", 4.0 * 1e-3 * 9.80665)
angular_velocity_stdev = rospy.get_param("~angular_velocity_stdev",
np.deg2rad(0.06))
linear_acceleration_cov = linear_acceleration_stdev * linear_acceleration_stdev
angular_velocity_cov = angular_velocity_stdev * angular_velocity_stdev
# From the UM7 datasheet for the dynamic accuracy from the EKF.
orientation_x_stdev = rospy.get_param("~orientation_x_stdev",
np.deg2rad(3.0))
orientation_y_stdev = rospy.get_param("~orientation_y_stdev",
np.deg2rad(3.0))
orientation_z_stdev = rospy.get_param("~orientation_z_stdev",
np.deg2rad(5.0))
orientation_x_covar = orientation_x_stdev * orientation_x_stdev
orientation_y_covar = orientation_y_stdev * orientation_y_stdev
orientation_z_covar = orientation_z_stdev * orientation_z_stdev
self.imu_data.orientation_covariance = [
orientation_x_covar, 0, 0, 0, orientation_y_covar, 0, 0, 0,
orientation_z_covar
]
self.imu_data.angular_velocity_covariance = [
angular_velocity_cov, 0, 0, 0, angular_velocity_cov, 0, 0, 0,
angular_velocity_cov
]
self.imu_data.linear_acceleration_covariance = [
linear_acceleration_cov, 0, 0, 0, linear_acceleration_cov, 0, 0, 0,
linear_acceleration_cov
]
# Set up the imu data message
self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=1)
# Set up the Roll-Pitch-Yaw (rpy) message
self.rpy_data = Vector3Stamped()
self.rpy_pub = rospy.Publisher('imu/rpy', Vector3Stamped, queue_size=1)
# Set up the magnometer message
self.mag_data = Vector3Stamped()
self.mag_pub = rospy.Publisher('imu/mag', Vector3Stamped, queue_size=1)
# what data to get from the UM7
self.statevars = [
'health', 'roll', 'pitch', 'yaw', 'mag_proc_x', 'mag_proc_y',
'mag_proc_z', 'accel_proc_x', 'accel_proc_y', 'accel_proc_z',
'gyro_proc_x', 'gyro_proc_y', 'gyro_proc_z', 'quat_a', 'quat_b',
'quat_c', 'quat_d', 'gps_latitude', 'gps_longitude', 'gps_altitude'
]
# Masks for parsing out the health data from the IMU
self.NUM_SATS_USED = 0b11111100000000000000000000000000
self.HDOP = 0b00000011111111110000000000000000 # Not used as of 05/23/19
self.NUM_SATS_IN_VIEW = 0b00000000000000001111110000000000
# Set up the GPS NavSatFix publisher
self.fix_data = NavSatFix()
self.fix_pub = rospy.Publisher('fix', NavSatFix, queue_size=1)
imu_connected = False
while not rospy.is_shutdown() and not imu_connected:
try:
self.driver = um7.UM7('s',
self.port,
self.statevars,
baud=115200)
imu_connected = True
rospy.loginfo("Imu initialization completed")
self.received = -1
if self.reset_mag:
self.driver.set_mag_reference()
if self.reset_accel:
cmd_seq.append(Um6Drv.CMD_SET_ACCEL_REF)
if self.mag_zero_x and self.mag_zero_y and self.mag_zero_z:
rospy.loginfo("Magnetometer calibration: %.3f %.3f %.3f",
self.mag_zero_x, self.mag_zero_y,
self.mag_zero_z)
except SerialException:
rospy.logwarn(
"Serial error communicating with IMU. Will retry in 2.0 seconds."
)
rospy.sleep(2.0)
# TODO: make action on the gripper as categorical policy
# action[-1] = reloc_rescale_gripper(action[-1])
next_obs, manager_reward, done = env.step_demo(
action, time_step=ep_len
) # reward R_t-> for high-level manager -> for sum(R_t:t+c-1)
randomize_world()
# update episodic logs
# Ignore the "done" signal if it comes from hitting the time
# horizon (that is, when it's an artificial terminal signal
# that isn't based on the agent's state) -> if done = False for the max_timestep
# DO NOT Make done = True when it hits timeout
ep_ret += manager_reward # reward in terms of achieving episodic task
done = False if ep_len == max_ep_len else done
if done:
rospy.logwarn(
'=============== Now epsiode %d ends with done signal! ====================',
episode_num)
next_meas_stt = np.concatenate(next_obs['observation']['meas_state'],
axis=0) # s_t
next_c_obs = next_obs['observation']['color_obs'] #o_t
next_aux_stt = np.concatenate(next_obs['observation']['auxiliary'],
axis=0) # s_aux
next_full_stt = np.concatenate([next_meas_stt, next_aux_stt], axis=0)
# append manager transition
manager_temp_transition[-1] = done # TODO verfiy
manager_temp_transition[-2] += manager_reward # sum(R_t:t+c)
manager_temp_transition[0].append(next_meas_stt) # append s_seq
manager_temp_transition[1].append(next_aux_stt) # append o_seq
manager_temp_transition[2].append(next_c_obs) # append a_seq
manager_temp_transition[3].append(action) # append a_seq
def gather_and_publish(self):
"""
Method that runs while ROS is up. It fetches, then processes the data
from the IMU. It only publishes the data when there are subscribers
"""
while not rospy.is_shutdown():
now = rospy.Time.now()
if (now.to_sec() - self.imu_data.header.stamp.to_sec()
) * self.throttle_rate < 1.0:
# Ignore data at this rate (ok for a boat)
return
else:
self.driver.catchallsamples(self.statevars, 0.1)
self.imu_data.header.stamp = now
# If someone is subscribed to these messages, then process the data and
# publish it. This avoids spending CPU cycles to process data no other
# nodes need
if self.imu_pub.get_num_connections() != 0:
self.imu_data.orientation = Quaternion()
# IMU outputs [w,x,y,z] NED, convert to [w, x, -y, -z] ENU in world frame
self.imu_data.orientation.w = self.driver.state['quat_a']
self.imu_data.orientation.x = self.driver.state['quat_b']
self.imu_data.orientation.y = -self.driver.state['quat_c']
self.imu_data.orientation.z = -self.driver.state['quat_d']
# convert to radians from degrees and scale according to values
# in the UM7 datasheet
# again note NED to ENU conversion
self.imu_data.angular_velocity.x = np.deg2rad(
self.driver.state['roll_rate'] / 16)
self.imu_data.angular_velocity.y = np.deg2rad(
-self.driver.state['pitch_rate'] / 16)
self.imu_data.angular_velocity.z = np.deg2rad(
-self.driver.state['yaw_rate'] / 16)
# again note NED to ENU conversion
self.imu_data.linear_acceleration.x = self.driver.state[
'accel_proc_x'] * GRAVITY # data['DATA_LINEAR_ACCEL'][1]
self.imu_data.linear_acceleration.y = -self.driver.state[
'accel_proc_y'] * GRAVITY # data['DATA_LINEAR_ACCEL'][0]
self.imu_data.linear_acceleration.z = -self.driver.state[
'accel_proc_z'] * GRAVITY # -(data['DATA_LINEAR_ACCEL'][2])
self.imu_pub.publish(self.imu_data)
# If someone is subscribed to these messages, then process the data and
# publish it. This avoids spending CPU cycles to process data no other
# nodes need
if self.rpy_pub.get_num_connections() != 0:
self.rpy_data.header = self.imu_data.header
self.rpy_data.vector.x = np.deg2rad(self.driver.state['roll'])
self.rpy_data.vector.y = np.deg2rad(
-self.driver.state['pitch'])
self.rpy_data.vector.z = np.deg2rad(-self.driver.state['yaw'])
self.rpy_pub.publish(self.rpy_data)
# If someone is subscribed to these messages, then process the data and
# publish it. This avoids spending CPU cycles to process data no other
# nodes need
if self.mag_pub.get_num_connections() != 0:
self.mag_data.header = self.imu_data.header
self.mag_data.vector.x = self.driver.state['mag_proc_y']
self.mag_data.vector.y = self.driver.state['mag_proc_x']
self.mag_data.vector.z = -self.driver.state['mag_proc_z']
self.mag_pub.publish(self.mag_data)
# Now, gather and process the GPS data
# If someone is subscribed to these messages, then process the data and
# publish it. This avoids spending CPU cycles to process data no other
# nodes need
if self.fix_pub.get_num_connections() != 0:
self.fix_data.header.stamp = rospy.get_rostime()
self.fix_data.header.frame_id = self.gps_frame_id
self.fix_data.status.service = NavSatStatus.SERVICE_GPS
# Mask the returned health register to determine the number of
# satellites in use.
self.number_of_sats_used = (
self.NUM_SATS_USED & self.driver.state['health']) >> 26
self.number_of_sats_inView = (
self.NUM_SATS_IN_VIEW & self.driver.state['health']) >> 10
# GPS needs at least 4 satellites to have a reliable fix
# So, if we don't have 4, we publish that we don't have a fix
if self.number_of_sats_used < 4:
self.fix_data.status.status = NavSatStatus.STATUS_NO_FIX
rospy.logwarn("No GPS fix.")
else:
self.fix_data.status.status = NavSatStatus.STATUS_FIX
# Publish the GPS data
# TODO: 05/23/19 - JEV - Should be publish NaN or 0 if we don't
# have a fix?
self.fix_data.latitude = self.driver.state['gps_latitude']
self.fix_data.longitude = self.driver.state['gps_longitude']
self.fix_data.altitude = self.driver.state['gps_altitude']
self.fix_data.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(self.fix_data)
def spin(self):
# state
s = self.sensor_state
odom = Odometry(header=rospy.Header(frame_id=self.odom_frame),
child_frame_id=self.base_frame)
js = JointState(name=[
"left_wheel_joint", "right_wheel_joint", "front_castor_joint",
"back_castor_joint"
],
position=[0, 0, 0, 0],
velocity=[0, 0, 0, 0],
effort=[0, 0, 0, 0])
r = rospy.Rate(self.update_rate)
last_cmd_vel = 0, 0
last_cmd_vel_time = rospy.get_rostime()
last_js_time = rospy.Time(0)
# We set the retry count to 0 initially to make sure that only
# if we received at least one sensor package, we are robust
# agains a few sensor read failures. For some strange reason,
# sensor read failures can occur when switching to full mode
# on the Roomba.
sensor_read_retry_count = 0
while not rospy.is_shutdown():
last_time = s.header.stamp
curr_time = rospy.get_rostime()
# SENSE/COMPUTE STATE
try:
self.sense(s)
transform = self.compute_odom(s, last_time, odom)
# Future-date the joint states so that we don't have
# to publish as frequently.
js.header.stamp = curr_time + rospy.Duration(1)
except select.error:
# packet read can get interrupted, restart loop to
# check for exit conditions
continue
except DriverError:
if sensor_read_retry_count > 0:
rospy.logwarn(
'Failed to read sensor package. %d retries left.' %
sensor_read_retry_count)
sensor_read_retry_count -= 1
continue
else:
raise
sensor_read_retry_count = self._SENSOR_READ_RETRY_COUNT
# Reboot Create if we detect that charging is necessary.
if s.charging_sources_available > 0 and \
s.oi_mode == 1 and \
s.charging_state in [0, 5] and \
s.charge < 0.93*s.capacity:
rospy.loginfo("going into soft-reboot and exiting driver")
self._robot_reboot()
rospy.loginfo("exiting driver")
break
# Reboot Create if we detect that battery is at critical level switch to passive mode.
if s.charging_sources_available > 0 and \
s.oi_mode == 3 and \
s.charging_state in [0, 5] and \
s.charge < 0.15*s.capacity:
rospy.loginfo("going into soft-reboot and exiting driver")
self._robot_reboot()
rospy.loginfo("exiting driver")
break
# PUBLISH STATE
self.sensor_state_pub.publish(s)
self.odom_pub.publish(odom)
if self.publish_tf:
self.publish_odometry_transform(odom)
# 1hz, future-dated joint state
if curr_time > last_js_time + rospy.Duration(1):
self.joint_states_pub.publish(js)
last_js_time = curr_time
self._diagnostics.publish(s, self._gyro)
if self._gyro:
self._gyro.publish(s, last_time)
# ACT
if self.req_cmd_vel is not None:
# check for velocity command and set the robot into full mode if not plugged in
if s.oi_mode != self.operate_mode and s.charging_sources_available != 1:
if self.operate_mode == 2:
self._robot_run_safe()
else:
self._robot_run_full()
# check for bumper contact and limit drive command
req_cmd_vel = self.check_bumpers(s, self.req_cmd_vel)
# Set to None so we know it's a new command
self.req_cmd_vel = None
# reset time for timeout
last_cmd_vel_time = last_time
else:
#zero commands on timeout
if last_time - last_cmd_vel_time > self.cmd_vel_timeout:
last_cmd_vel = 0, 0
# double check bumpers
req_cmd_vel = self.check_bumpers(s, last_cmd_vel)
# send command
self.drive_cmd(*req_cmd_vel)
# record command
last_cmd_vel = req_cmd_vel
r.sleep()
