def send_command(self):
id = 68
msg = can.Message(arbitration_id=id,
extended_id=False,
data=[0, 0, 0, 0, 0, 0, 0,
0]) # 11-bit identifier (not-extended)
if self.verbose:
rospy.logwarn(
"Sending initialization message (ID: {})!".format(id))
self.bus.send(msg)
if self.verbose: rospy.info(msg)
def pose_diff_cb(pose_stamped):
global teacher_pose_stamped, renew_flag
# add diff and pub
if (not teacher_pose_stamped):
rospy.info ("teacher is empty")
return
# DummyArrayPub.publish(PointsArray()) # register empty clouds to stop recognition
try:
teacher_pose_stamped.header.stamp = rospy.Time(0)
teacher_pose_stamped_recog_frame = listener.transformPose(pose_stamped.header.frame_id, teacher_pose_stamped)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception), e:
print "tf error: %s" % e
return
def setSlope(self, slope=255):
try:
rospy.info("Setting controller slopes to: {}".format(slope))
rospy.wait_for_service('/neck/pan_controller/set_compliance_slope')
rospy.wait_for_service(
'/neck/tilt_right_controller/set_compliance_slope')
rospy.wait_for_service(
'/neck/tilt_left_controller/set_compliance_slope')
self.pan_slope(slope)
self.tilt_left_slope(slope)
self.tilt_right_slope(slope)
except Exception as e:
rospy.logerr("Failed setting slope service. Reason: {}".format(e))
def pose_diff_cb(pose_stamped):
global teacher_pose_stamped, renew_flag
# add diff and pub
if (not teacher_pose_stamped):
rospy.info("teacher is empty")
return
# DummyArrayPub.publish(PointsArray()) # register empty clouds to stop recognition
try:
teacher_pose_stamped.header.stamp = rospy.Time(0)
teacher_pose_stamped_recog_frame = listener.transformPose(
pose_stamped.header.frame_id, teacher_pose_stamped)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, tf.Exception), e:
print "tf error: %s" % e
return
def _connect(self):
print("Connecting...")
while not rospy.is_shutdown():
startup = "\xAA\x02\x01\x01\xFD"
self.uart.write(startup)
recv = serial.read(256)
if recv[0] == "\xAA" and recv[6] == "\xAC":
rospy.info("Connected!")
break
self.r.sleep()
def __init__(self, controller_name='mpc'):
# super(offboard_controller, self).__init__() # only workable for python3
# mavros_core_function.__init__(self, ) # handle default mavros info
self.last_request = None
self.state = State()
self.extended_state = ExtendedState()
## Topics
self.local_pos_pub = rospy.Publisher('mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
rospy.Subscriber('/mavros/state', State, self.mavros_state_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
## Service
self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('/mavros/set_mode', SetMode)
## Timer
self.rate = rospy.Rate(50)
if controller_name == 'pid':
rospy.wait_for_service('uav_pid_server')
self.control_state_check_function = rospy.ServiceProxy(
'uav_pid_server', SetBool)
self.controller_state = self.control_state_check_function(True)
# print(self.controller_state)
elif controller_name == 'mpc':
rospy.wait_for_service('uav_mpc_server')
self.control_state_check_function = rospy.ServiceProxy(
'uav_mpc_server', SetBool)
self.controller_state = self.control_state_check_function(True)
else:
rospy.info("Unknown controller")
# self.set_local_position()
if self.controller_state.success:
# setup the arm and ready for the flight
# self.set_arm(True, 10)
self.offboard_ready = True
else:
rospy.loginfo(
'UAV controller server is not ready yet, please launch a controller first'
)
self.offboard_ready = False
def check_fcu():
try:
state = rospy.wait_for_message('mavros/state', State, timeout=3)
if not state.connected:
failure('no connection to the FCU (check wiring)')
return
# Make sure the console is available to us
mavlink_exec('\n')
version_str = mavlink_exec('ver all')
if version_str == '':
rospy.info('No version data available from SITL')
r = re.compile(r'^FW (git tag|version): (v?\d\.\d\.\d.*)$')
is_clever_firmware = False
for ver_line in version_str.split('\n'):
match = r.search(ver_line)
if match is not None:
field, version = match.groups()
rospy.loginfo('FCU firmware %s: %s' % (field, version))
if 'clever' in version:
is_clever_firmware = True
if not is_clever_firmware:
failure(
'Not running Clever PX4 firmware, check http://clever.copterexpress.com/firmware.html'
)
est = get_param('SYS_MC_EST_GROUP')
if est == 1:
rospy.loginfo('Selected estimator: LPE')
fuse = get_param('LPE_FUSION')
if fuse & (1 << 4):
rospy.loginfo('LPE_FUSION: land detector fusion is enabled')
else:
rospy.loginfo('LPE_FUSION: land detector fusion is disabled')
if fuse & (1 << 7):
rospy.loginfo('LPE_FUSION: barometer fusion is enabled')
else:
rospy.loginfo('LPE_FUSION: barometer fusion is disabled')
elif est == 2:
rospy.loginfo('Selected estimator: EKF2')
else:
failure('Unknown selected estimator: %s', est)
except rospy.ROSException:
failure('no MAVROS state (check wiring)')
def choose_path(self, node):
if (self.G.node[node]['ip'] != 0):
request = self.G.node[node]['ip']
print("A interseção retornada é: " + str(request))
self.G.node[node]['ip'] -= 1
return request
request = 0
length_min = 0
aux = 0
target = 0
rospy.info("The lengths from the actual node " + str(node) +
" from targets are: ")
for index in range(self.non):
if (self.G.node[index]['ip'] != 0 and index != node):
aux = nx.dijkstra_path_length(self.G,
node,
index,
weight='weight')
print(str(aux) + " : to node " + str(index))
if (aux < length_min or length_min == 0):
length_min = aux
target = index
if (length_min != 0):
rospy.loginfo("The target is: " + str(target))
shortest_path = nx.dijkstra_path(self.G,
node,
target,
weight='weight')
print("The shortest path's length is: " + str(length_min))
print(shortest_path)
request = self.nav_path(shortest_path, node)
elif (length_min == 0):
self.map_completed = True
for index in range(self.non):
neighbors = self.G.neighbors(index)
if (neighbors <= 1):
self.map_completed = False
break
if (self.map_completed):
rospy.loginfo("The mapping is finnish")
return request
def go_to_position_handle(req):
"""
The handle for arm moving to desired position.
Go_To_Position.srv:
# go to desired position
geometry_msgs/Pose pose
---
# IK service result
std_msgs/Bool ik_flag
# action result flag
std_msgs/Bool action_flag
"""
_pose = copy.deepcopy(req.pose)
joint_angles = pnp.ik_request(_pose)
resp = Go_To_PositionResponse()
print "Endeffector Destination Pose x=%f y=%f z=%f ox=%f oy=%f oz=%f ow=%f" % (
req.pose.position.x, req.pose.position.y, req.pose.position.z,
req.pose.orientation.x, req.pose.orientation.y, req.pose.orientation.z,
req.pose.orientation.w)
if joint_angles:
resp.ik_flag.data = True
time_prev = rospy.Time.now()
pnp._guarded_move_to_joint_position(joint_angles)
action_duration = rospy.Time.now() - time_prev
print "Moving Action Time Duration %f" % action_duration.to_sec()
if action_duration.to_sec() >= 7.0:
resp.action_flag.data = False
else:
resp.action_flag.data = True
else:
resp.ik_flag.data = False
if not resp.ik_flag:
rospy.info("IK Solution Check not pass!")
if not resp.action_flag:
rospy.info("Arm Motion did not complete for timeout!")
return resp
def run_task(self): ## move this function to robot's task worker
"""Access the task queue and execute in the robot
"""
# Request task list
lastWp = []
# TODO: Make it a infinity loop
for task in self.plan.task_list:
if task.action == 'move':
rospy.loginfo('Move to [%s]', task.target)
self.__move_task(task.robot, task.target)
lastWP = self.waypoints[task.target]
elif task.action == 'pick':
rospy.loginfo('Moving to [%s] at [%s]', task.product,
task.target)
rospy.loginfo('From [%s] \n to [%s]', lastWP,
self.waypoint2pick(lastWP, task.product))
self.__move_pose(self.waypoint2pick(lastWP, task.product))
product_name = self.add_product(task.target, task.product)
rospy.loginfo('Picking [%s] at [%s]', task.product,
task.target)
input()
self.__pick_task(product_name)
store = self.__available_storage()
if not store:
rospy.info("No available Store")
else:
rospy.loginfo("Storing product on [%s]", store[0])
res = self.__place_task(store[0])
store[1] = 1
elif task.action == 'drop':
rospy.loginfo('Dropping [%s] at [%s]', task.product,
task.target)
self.scene.remove_attached_object('base_footprint',
task.product)
self.__pick_task(task.product)
rospy.loginfo('Dropping at position: [%s]',
self.drop_position(task.target))
self.__place_task(self.drop_position(task.target))
def grab_url(self, window):
"""
given a window (from the director message) grab either the kiosk or
the display current url based on the viewport from the window. Apply
any tactile changes if maps.google.com is part of the url
"""
url_service = None
activity = window.get('activity', None)
if activity != 'browser':
return
viewport = window.get('presentation_viewport', None)
if viewport is None:
rospy.info("viewport was None... ignoring")
return
# display might be ok to go away, but only once we're sure
if viewport != 'kiosk' and viewport != 'wall' and viewport != 'display':
rospy.warn("Unable to determine viewport named (%s)" % viewport)
return
if viewport == 'kiosk':
url_service = self.kiosk_url_service
elif viewport == 'display' or viewport == 'wall':
url_service = self.display_url_service
state = url_service.call().state
try:
state = json.loads(state)
except:
rospy.logwarn("Unable to parse state (%s)" % state)
raise
if len(state) > 1:
rospy.logwarn('There are more than one browser active, the wrong URL might be returned')
for browser_id, browser_data in state.iteritems():
return browser_data['current_url_normalized']
rospy.logdebug("New image mode requested was already the image mode.")
def _startTimer(self):
"""
This function shuts down the timer if it has been running, prints the
first image in the sequence (by resetting _imageUpdate()) and then
starts the timer with the stored period.
"""
rospy.logdebug("Calling _startTimer")
if self.image_timer:
self.image_timer.shutdown()
# Note that the timer won't show an image until the timer goes off, so
# let's call the function that the callback calls once
# Note that we only set up a timer if the period is > 0
self._imageUpdate(reset=True)
if self._image_period > 0:
self.image_timer = rospy.Timer(rospy.Duration(self._image_period),self.timerCallback)
if __name__=="__main__":
#Simple test script for ImageSwitcher class
rospy.init_node('image_switcher_test_node', log_level=rospy.INFO)
rospy.info("image switcher timer test")
img_sw = ImageSwitcher('top')
rospy.info("made object")
rospy.sleep(6)
img_sw.change_mode('crane_prep')
rospy.spin()
def throw_away_tasks(self, all_tasks, amount):
'''This method throw away tasks based on priorities, or if tasks have same priority, than amount parameter is applied.
Returns true if tasks were throwen away (have lower prio than active_priority), ortherwise return False'''
amount_tasks = len(all_tasks)
if (amount_tasks == 0):
rospy.info(
'EXECUTOR: Throwing of tasks called but there is nothing to throw away.'
)
return [], [], False
all_tasks.sort(key=attrgetter('priority'),
reverse=True) # highest priority first
low_prio = all_tasks[amount_tasks - 1] # get the lowest prio
index = all_tasks.index(low_prio) # find first occurencce of low_prio
low_prio_amount = amount_tasks - index #get how many tasks have the lowest prio
throw_num = floor(amount * amount_tasks) # compute amount percentage
if (throw_num < 1): #throw always at least one
throw_num = 1
throwen_away = []
sub_additional = [] # tasks what will be added
throw_num = min([low_prio_amount, throw_num
]) # choose smaller amount of tasks to throw away
throw_index = amount_tasks - throw_num
if len(self.active_tasks) > 0:
# for the scheduled_task_execution active_tasks will contain at most 1 task
if (low_prio.priority <= self.active_tasks[0].priority
): #we want to throw away tasks
rospy.loginfo(
'Schedule not found, trying to discard %s tasks with priority %s',
str(throw_num), str(low_prio.priority))
#if there are more low priority task, only amount% will be throwen away...
for i in range(0, amount_tasks):
if i < throw_index:
sub_additional.append(all_tasks[i])
else:
throwen_away.append(all_tasks[i])
return sub_additional, throwen_away, True
else: #task which should be throwen away has higher priority then active task
if len(
self.active_tasks
) > 0 and not self.is_task_interruptible(
self.active_tasks[0]
): #we cant preemt, thus returninf True and throwing away task
rospy.loginfo('Active task cannot be preempted.')
rospy.loginfo(
'Schedule not found, trying to discard %s tasks with priority higher prio %s',
str(throw_num), str(low_prio.priority))
for i in range(0, amount_tasks):
if i < throw_index:
sub_additional.append(all_tasks[i])
else:
throwen_away.append(all_tasks[i])
return sub_additional, throwen_away, True
else: #task is interruptible, we want to preempt -> return False
return all_tasks, [], False
else: #nothing running yet
rospy.loginfo(
'Schedule not found, trying to discard %s tasks with priority %s',
str(throw_num), str(low_prio.priority))
#if there are more low priority task, only amount% will be throwen away...
for i in range(0, amount_tasks):
if i < throw_index:
sub_additional.append(all_tasks[i])
else:
throwen_away.append(all_tasks[i])
return sub_additional, throwen_away, True
def start_goal(self):
rospy.info('Mock say something: {}'.format(self._speech))
while time.time() - time_tra <= 0.8:
angle_car.publish(-60)
bamlan = 1
speed.publish(20)
steer_bool = -1
# if sign_bool == 3:
# bamlan = 1
if sign_bool == 4: #noleft
sign_bool = 69
if speed_up_no(road_on_birdview) > 100: # di thang
time_nl_t = time.time()
while time.time() - time_nl_t <= 0.6:
#print(time.time() - time_t)
angle_car.publish(0)
#print("fuckkkkkkkk")
rospy.info("TURN STRAIGHT!!!")
#sign_bool= 66
steer_bool = -1
else: #re phai
# print("HERERJEIRJRJKWQNJKASBDJKASDBAHJDHJWAHJD213123213")
print("TURN RIGHT on NOLEFT!!!")
time_nl_r = time.time()
while time.time() - time_nl_r <= 1:
#print("TURN RIGHT \t",time.time() - time_nl_r)
angle_car.publish(-60)
#print("f**k")
#sign_bool= 66
speed.publish(20)
steer_bool = -1
# if sign_bool == 5:#no right
# sign_bool = 1196
return 1.0 / (1 + np.exp(-x))
rospy.init_node("musctest")
p = rospy.Publisher('/roboy/middleware/MotorCommand',
MotorCommand,
queue_size=1)
rospy.loginfo("Changing control mode to DISPLACEMENT")
change_control_mode = rospy.ServiceProxy(
'/roboy/shoulder_left/middleware/ControlMode', ControlMode)
req = ControlModeRequest()
req.motor_id = [0, 1, 2, 3]
req.control_mode = 2 # DISPLACEMENT
change_control_mode(req)
rospy.info("Changed to DISPLACEMENT control mode")
max_cmd = [100, 20]
msg = MotorCommand()
msg.id = 3
def encoderticks2degrees(ticks):
return ticks * (360.0 / 4096.0)
center = [2673.5, 2539.0]
motors_map = {0: [0, 1], 1: [2, 3]}
def cb2(data):
# for camera_id in id_list"
# camera_publisher = rospy.Publisher('/carla/{}/image'.format(camera_id),
# Image,
# queue_size=1)
# self.publisher_list.append(camera_publisher)
if __name__ == '__main__':
client = carla.Client('localhost', 2001)
client.set_timeout(10.0)
world = client.get_world()
world_snapshot = world.get_snapshot()
cameras=[actor for actor in actual_actor if actor.type_id.find('camera')!=-1]
rospy.info("find {} camera sensors!".format(len(cameras)))
#getGT_node = GetGroundTruth(cameras)
try:
while not rospy.core.is_shutdown():
world_snapshot = world.get_snapshot()
actual_actor=[world.get_actor(actor_snapshot.id) for actor_snapshot in world_snapshot]
vehicles=[actor for actor in actual_actor if actor.type_id.find('vehicle')!=-1]
loc_vehicles_dic = {}
for cam in cameras:
loc_vehicles = [vehicle for vehicle in vehicles if vehicle.get_transform().location.distance(cam.get_transform().location)<80]
loc_vehicles_dic[cam.id] = loc_vehicles
for key,value in loc_vehicles_dic:
print(key,": ",len(value))
w.command = item['command']
w.autocontinue = item['autoContinue']
w.param1 = item['param1']
w.param2 = item['param2']
w.param3 = item['param3']
w.param4 = item['param4']
w.x_lat = item['coordinate'][0]
w.y_long = item['coordinate'][1]
w.z_alt = item['coordinate'][2]
yield w
if __name__ == '__main__':
rospy.init_node('mission_loader', anonymous=True)
if 'MISSION_HASH' in os.environ and len(os.environ['MISSION_HASH']) > 0:
mission = os.environ['MISSION_HASH'].strip()
rospy.wait_for_service('mavros/mission/push')
push = rospy.ServiceProxy('mavros/mission/push', WaypointPush)
rospy.info('Try to load mission {0}'.format(mission))
route = Route(mission)
try:
route.acquire()
push(list(get_mission(mission)))
rospy.loginfo('Route {0} acquired and loaded'.format(mission))
except:
route.release()
rospy.logerror('Unable to load route {0}'.format(mission))
else:
rospy.logwarn('No mission specified, skip loading')
self.reverse()
rospy.sleep(self.sleep_duration)
def start_random_walk(self):
self.thread.start()
if __name__ == "__main__":
robots = []
for i in range(1, 11):
robo_name = "robot{}".format(i)
robot = RandomWalker(name=robo_name, lin_vel=0.9, ang_vel=1)
robot.start_random_walk()
robots.append(robot)
try:
i = 10
while i > 0:
rospy.sleep(1)
i -= 1
except Exception as err:
[robot.__del__() for robot in robots]
rospy.info(err)
finally:
[robot.__del__() for robot in robots]
rospy.logdebug("New image mode requested was already the image mode.")
def _startTimer(self):
"""
This function shuts down the timer if it has been running, prints the
first image in the sequence (by resetting _imageUpdate()) and then
starts the timer with the stored period.
"""
rospy.logdebug("Calling _startTimer")
if self.image_timer:
self.image_timer.shutdown()
# Note that the timer won't show an image until the timer goes off, so
# let's call the function that the callback calls once
# Note that we only set up a timer if the period is > 0
self._imageUpdate(reset=True)
if self._image_period > 0:
self.image_timer = rospy.Timer(rospy.Duration(self._image_period),self.timerCallback)
if __name__=="__main__":
#Simple test script for ImageSwitcher class
rospy.init_node('image_switcher_test_node', log_level=rospy.INFO)
rospy.info("image switcher timer test")
img_sw = ImageSwitcher('top')
rospy.info("made object")
rospy.sleep(6)
img_sw.change_mode('crane_prep')
rospy.spin()
cotresponse = takserver.readcot(
readtimeout=1) # This is a blocking read for 1 second.
cot_xml = cotresponse[0]
rospy.loginfo("COT XML:\n%s" % (cot_xml))
except:
rospy.logdebug("Read Cot failed: %s" % (sys.exc_info()[0]))
#continue
# Parse a received message and if it is a move to command publish a move to message
try:
tree = ET.fromstring(cot_xml)
# Get the UID
this_uid = tree.get("uid")
fiveline = tree.find("./detail/fiveline")
target_num = fiveline.attrib['fiveline_target_number']
rospy.info("TAKMSG:-----\n\n%s" + str(tree))
if ('99999' == target_num
): # TODO change to publish a go to goal message
lat = tree.find("./point").attrib['lat']
lon = tree.find("./point").attrib['lon']
rospy.loginfo(
"----- Recieved ATAK Message from UID: %s, saying move to lat/lon of %s, %s"
% (this_uid, lat, lon))
except Exception, e:
rospy.logdebug(
"----- Recieved ATAK Message and it is not a move to command -----"
+ str(e))
count += 1
if count > 20: # ping server once a second to keep alive.
rospy.logdebug(
