def get_data_from_controller():
while not done:
rospy.init_node('get_data_from_controller')
rospy.Subscriber("position_correction", String, callback)
rospy.spin()
rospy.signal_shutdown("Shutting Down")
def main():
global client
try:
rospy.init_node("test_move", anonymous=True, disable_signals=True)
client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
print "Waiting for server..."
client.wait_for_server()
print "Connected to server"
while 1:
#move1()
#move2()
#print 'Enter Tra Array:'
#s = raw_input()
#split1 = s.split(" ")
#for i in split1:
# Q.append(float(i))
#move1()
#del Q[0:len(Q)]
#move1()
#move_repeated()
#move_disordered()
#move_interrupt()
#move1()
#move_repeated()
#move_disordered()
#move_interrupt()
except KeyboardInterrupt:
rospy.signal_shutdown("KeyboardInterrupt")
raise
def transition(time, next_state):
"""Transition function to help state machine."""
#Check if wheels have dropped
if D.data.wheeldropCaster:
print "Wheel drop!"
print "Shutting down..."
Robo.move(0,0)
Robo.mode = "ambivalent"
Robo.status = "shutdown"
rospy.signal_shutdown("robot picked up... so we're shutting down")
#Check for incoming messages from the hive
hiveCommand = check_hive_updates()
Robo.hiveCommand = hiveCommand
#if Robo.hiveCommmand == "stop":
# rospy.Timer(rospy.Duration(time), state_wait, oneshot=True)
print Robo.curState
#Publish robot status updates
update = Robo.status
rospy.loginfo(update)
status_updates.publish(String(update))
rospy.Timer(rospy.Duration(time), next_state, oneshot=True)
def _shutdown_by_reactor(self):
"""
Reactor shutdown callback. Sends a signal to rospy to shutdown the ROS
interfaces
"""
rospy.signal_shutdown("Reactor shutting down.")
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 on_key(event):
'''Quit if the q key was pressed'''
global plt
if(event.key == 'q'):
plt.close('all') # Close all figures
rospy.signal_shutdown('Quitting...') # Ask ROS to shutdown
def __init__(self, port=None, baud=57600, timeout=5.0):
""" Initialize node, connect to bus, attempt to negotiate topics. """
self.mutex = thread.allocate_lock()
self.lastsync = rospy.Time(0)
self.lastsync_lost = rospy.Time(0)
self.timeout = timeout
self.synced = False
self.pub_diagnostics = rospy.Publisher('/diagnostics', diagnostic_msgs.msg.DiagnosticArray)
if port== None:
# no port specified, listen for any new port?
pass
elif hasattr(port, 'read'):
#assume its a filelike object
self.port=port
else:
# open a specific port
try:
self.port = Serial(port, baud, timeout=self.timeout*0.5)
except SerialException as e:
rospy.logerr("Error opening serial: %s", e)
rospy.signal_shutdown("Error opening serial: %s" % e)
raise SystemExit
self.port.timeout = 0.01 # Edit the port timeout
time.sleep(0.1) # Wait for ready (patch for Uno)
# hydro introduces protocol ver2 which must match node_handle.h
# The protocol version is sent as the 2nd sync byte emitted by each end
self.protocol_ver1 = '\xff'
self.protocol_ver2 = '\xfe'
self.protocol_ver = self.protocol_ver2
self.publishers = dict() # id:Publishers
self.subscribers = dict() # topic:Subscriber
self.services = dict() # topic:Service
self.buffer_out = -1
self.buffer_in = -1
self.callbacks = dict()
# endpoints for creating new pubs/subs
self.callbacks[TopicInfo.ID_PUBLISHER] = self.setupPublisher
self.callbacks[TopicInfo.ID_SUBSCRIBER] = self.setupSubscriber
# service client/servers have 2 creation endpoints (a publisher and a subscriber)
self.callbacks[TopicInfo.ID_SERVICE_SERVER+TopicInfo.ID_PUBLISHER] = self.setupServiceServerPublisher
self.callbacks[TopicInfo.ID_SERVICE_SERVER+TopicInfo.ID_SUBSCRIBER] = self.setupServiceServerSubscriber
self.callbacks[TopicInfo.ID_SERVICE_CLIENT+TopicInfo.ID_PUBLISHER] = self.setupServiceClientPublisher
self.callbacks[TopicInfo.ID_SERVICE_CLIENT+TopicInfo.ID_SUBSCRIBER] = self.setupServiceClientSubscriber
# custom endpoints
self.callbacks[TopicInfo.ID_PARAMETER_REQUEST] = self.handleParameterRequest
self.callbacks[TopicInfo.ID_LOG] = self.handleLoggingRequest
self.callbacks[TopicInfo.ID_TIME] = self.handleTimeRequest
rospy.sleep(2.0) # TODO
self.requestTopics()
self.lastsync = rospy.Time.now()
def get_param_duration(param):
"""Calls rospy.get_param and logs errors.
Logs if the param does not exist or is not parsable to rospy.Durotation.
And calls rospy.signal_shutdown if the value is invalid/not existing.
:return: The Param param from the parameter server.
:rtype: rospy.Duration
"""
# dummy value
value = rospy.Duration(1)
try:
# only a default value in case the param gets fuzzed.
value = rospy.Duration(get_param_num(param))
except ValueError:
err_msg = (
"Param %s has the invalid value '%s'."
% (param, rospy.get_param(param)))
rospy.logerr(err_msg)
rospy.signal_shutdown(err_msg)
value = rospy.Duration(1)
return value
def checkImage(self):
path2 = "test"
test_images = [os.path.join(path2, item) for item in os.listdir(path2)]
for im_path in test_images:
curr_image_gray = PIL.Image.open(im_path).convert('L')
np_array = np.array(curr_image_gray , 'uint8')
all_faces = self.fC.detectMultiScale(np_array)
#print all_faces
for (x , y , width , height) in all_faces:
pred_value, confidence = self.predictor.predict(np_array[y: y + height, x: x + width])
if self.unlocked == True:
break
if confidence >= 10:
self.unlocked = True
print "UNLOCKED *****************************************************************************"
self.connection.write(self.beep_command)
break
if self.unlocked == False:
print "LOCKED ******************************** Shutting Down"
rospy.signal_shutdown(1)
def main(argv):
app = wx.App()
KeyEventFrame()
print(__doc__)
app.MainLoop()
rospy.signal_shutdown("MainLoop")
return 0
def window_shutdown(self, widget):
# kill gtk thread
gtk.main_quit()
# kill roscomms thread
self.roscommsThread.join(0) # I think it is dead!
# kill ros thread
rospy.signal_shutdown("Because I said so!")
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
global client
try:
client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
print "Waiting for server..."
client.wait_for_server()
print "Connected to server"
except KeyboardInterrupt:
rospy.signal_shutdown("KeyboardInterrupt")
raise
receive()
#global sub_stp
#sub_stp = rospy.Subscriber("chatter", String, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def main():
""" the main function that sets everything up
"""
# Initialize this ROS node
rospy.init_node("kbd_publisher") # any name will do
# create our window - be sure the window has the focus!
windows_init()
# create a String publisher (pub)
pub = rospy.Publisher("text_data", String)
# main loop
while rospy.is_shutdown() == False:
# Get incoming key events using cv.WaitKey (lowest 8 bits)
key_code = cv.WaitKey(10) & 255 # gets the window's next key_code
if key_code == 255:
continue # 255 means "no key pressed"
key_press = chr(key_code) # convert key_code to a character
if " " <= key_press <= "z": # if it's in our valid range
print "Publishing ", str(key_press)
pub.publish(String(str(key_press))) # publish!
if key_code == 27 or key_press == "q": # if ESC or 'q' was pressed
pub.publish(String("q")) # publish the string 'q' either way
rospy.signal_shutdown("Quitting...")
print "Bye!"
def __init__(self):
self.config = read_config()
self.finished_pub = rospy.Publisher("/map_node/sim_complete", Bool ,queue_size=20)
self.path_list = rospy.Publisher("/results/path_list", AStarPath ,queue_size=30)
self.policy_list = rospy.Publisher("/results/policy_list", PolicyList ,queue_size=100)
rospy.init_node('robot',anonymous=True)
self.rate = rospy.Rate(1)
rospy.sleep(1)
# move_list = self.config["move_list"]
# map_size = self.config["map_size"]
# start = self.config["start"]
# goal = self.config["goal"]
# walls = self.config["walls"]
# pits = self.config["pits"]
# cost = self.config["reward_for_each_step"]
# a_star = astar(move_list,map_size,start,goal,walls,pits,cost)
# mdp_policies = mdp(self.config)
reinforcement = ReinforcementLearning(self.config)
# # for move in a_star:
# # self.path_list.publish(move)
# rospy.sleep(1)
# for policy in mdp_policies:
# self.policy_list.publish(mdp_policies[len(mdp_policies)-1])
for i in range(1000):
self.policy_list.publish(reinforcement.allPolicies[999])
# rospy.sleep(1)
rospy.sleep(1)
self.finished_pub.publish(True)
rospy.sleep(1)
rospy.signal_shutdown("finished moving")
def main():
rospy.init_node('axclient', anonymous=True)
parser = OptionParser(__doc__.strip())
# parser.add_option("-t","--test",action="store_true", dest="test",default=False,
# help="A testing flag")
# parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah")
(options, args) = parser.parse_args(rospy.myargv())
if (len(args) != 2):
parser.error("You must specify the action topic name Eg: ./axclient.py my_action_topic")
# get action type from topic
topic_type = rostopic._get_topic_type("%s/goal"%args[1])[0]
# remove "Goal" string from action type
assert("Goal" in topic_type)
topic_type = topic_type[0:len(topic_type)-4]
action = DynamicAction(topic_type)
app = AXClientApp(action, args[1])
app.MainLoop()
app.OnQuit()
rospy.signal_shutdown('GUI shutdown')
def tear_down(self, keep_alive=False):
# forcefully shut down service before killing node
if self.dserver:
self.dserver.set_service.shutdown("User deleted template.")
# for rostest (and potentially other cases), we want to clean up but keep the node alive
if not keep_alive:
rospy.signal_shutdown("User deleted template.")
def run(self, _):
try:
for container in self._containers:
self._conn.createContainer(**container)
for node in self._nodes:
self._conn.addNode(**node)
for parameter in self._parameters:
self._conn.addParameter(**parameter)
for interface in self._interfaces:
self._conn.addInterface(**interface)
for connection in self._connections:
self._conn.addConnection(**connection)
for ros in self._interfaces:
iType = ros['iType']
if iType in _MAP:
self._ifs.append(
getattr(self._conn, _MAP[iType])(ros['iTag'],
ros['iCls'],
ros['addr'])
)
except Exception as e:
import traceback
print(''.join(traceback.format_exception_only(type(e), e)))
rospy.signal_shutdown('Error')
def go(self, *args, **kwargs):
r = rospy.Rate(20)
count = 0
while not rospy.is_shutdown():
c = self.stdscr.getch()
if c == ord('q'):
self.curses_msg("Shutting down...")
self.restore_screen()
rospy.signal_shutdown("Quitting..")
elif c == ord('r'):
self.reset()
elif c == curses.KEY_RIGHT:
self.cur_page = (self.cur_page + 1) % self.num_pages
elif c == curses.KEY_LEFT:
self.cur_page = (self.cur_page - 1) % self.num_pages
else:
# Page-specific keys:
if self.cur_page == self.page_names.index("Control"):
if c >= ord('1') and c <= ord('9'):
self.mode_to_active(c - ord('1'))
if self.cur_page == self.page_names.index('Mode'):
cur_mode = self.latest.controller_status.active_mode
if cur_mode == "hover":
if c == ord('m'):
self.toggle_modify_hover()
elif c == ord('y'):
self.toggle_modify_yaw_in_hover()
# Refresh display:
count = (count + 1) % 4
if count == 0 or (self.cur_page != self.prev_page):
self.update()
r.sleep()
def adjust_yn():
def yes():
done = True
rospy.signal_shutdown("Shutting Down")
def no():
main_menu()
bindings = {
# key: (function, args, description)
'y': (yes,[], "Let Robot Continue"),
'n': (no, [], "Enter Control Mode"),
}
done = False
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Shutting Down.")
elif c in bindings:
cmd = bindings[c]
cmd[0](*cmd[1])
else:
print("key bindings: ")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
def locator(self):
while not rospy.is_shutdown():
"Waiting"
try:
image = np.asarray(cv2.cv.CloneMat(self.image))
except AttributeError:
rospy.sleep(1)
continue
image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
image = cv2.GaussianBlur(image, (3,3), 2)
edges = cv2.Canny(image, 50, 20)
#c = edges
#c = cv2.HoughCircles(edges, cv2. , 2, 100)
circles = cv2.HoughCircles(image, cv2.cv.CV_HOUGH_GRADIENT, 2, 20, maxRadius=100)
print circles
if circles is not None:
for c in circles[0]:
cv2.circle(image, (c[0],c[1]), c[2], (0,255,0),2)
#convert to HSV space
#image = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
#Filter for yellow balls
#chan = cv.CloneMat(image)
#chan = cv2.split(np.asarray(image))
#cv.Threshold(image,image,228,255,cv.CV_THRESH_BINARY)
#cv.Dilate(image,image,None,2)
#cv.Erode(image,image,None,2)
cv2.imshow("Image window", image)
cv2.imshow("Edges", edges)
if cv2.waitKey(2) == 1048603:
rospy.signal_shutdown("You asked me to you bastard")
rospy.sleep(0.05)# sleeping for 50 ms
def process_depth(depth):
#depth_image = D.bridge.imgmsg_to_cv2(data, "32FC1").astype(np.uint8)
#cv2.imshow('im', depth)
#try:
# depth_image = cv2.warpPerspective(depth_image, H, (config.GAME_WIDTH,config.GAME_HEIGHT))
#except NameError:
# pass
center = findSphero_depth(depth)
if not center:
print 'None'
D.pub.publish(str(center))
#depth_image = depth.astype(np.uint8)
#cv2.circle(depth_image, center, 5, (255, 0, 0), 2)
#cv2.imshow('im', depth_image)
key_press = cv2.waitKey(5) & 0xff
if key_press == 27 or key_press == ord('q'):
rospy.signal_shutdown(0)
def command_callback(self, command):
self.command = command.data
if (self.command == 'exit'):
self.pubPanelsCommand.publish(MsgPanelsCommand(command='stop', arg1=0, arg2=0, arg3=0, arg4=0, arg5=0, arg6=0))
self.bRunning = False
rospy.signal_shutdown('User requested exit.')
if (self.command == 'stop'):
self.pubPanelsCommand.publish(MsgPanelsCommand(command='stop', arg1=0, arg2=0, arg3=0, arg4=0, arg5=0, arg6=0))
self.pubPanelsCommand.publish(MsgPanelsCommand(command='all_off', arg1=0, arg2=0, arg3=0, arg4=0, arg5=0, arg6=0))
self.bRunning = False
if (self.command == 'start'):
self.pubPanelsCommand.publish(MsgPanelsCommand(command='set_pattern_id', arg1=self.params['pattern_id'], arg2=0, arg3=0, arg4=0, arg5=0, arg6=0))
self.pubPanelsCommand.publish(MsgPanelsCommand(command='start', arg1=0, arg2=0, arg3=0, arg4=0, arg5=0, arg6=0))
self.bRunning = True
if (self.command == 'help'):
rospy.logwarn('The %s/command topic accepts the following string commands:' % self.nodename.rstrip('/'))
rospy.logwarn(' help This message.')
rospy.logwarn(' stop Stop the ledpanels.')
rospy.logwarn(' start Start the ledpanels.')
rospy.logwarn(' exit Exit the program.')
rospy.logwarn('')
rospy.logwarn('You can send the above commands at the shell prompt via:')
rospy.logwarn('rostopic pub -1 %s/command std_msgs/String commandtext' % self.nodename.rstrip('/'))
rospy.logwarn('')
rospy.logwarn('Parameters are settable as launch-time parameters.')
rospy.logwarn('')
def main():
pub = rospy.Publisher("/vesselness/settings", vesselness_params, queue_size=10)
rospy.init_node("vesselness_filter_parameter_gui", anonymous=True)
rate = rospy.Rate(1) # 10hz
# Initialize parameters
params = vesselness_params()
# Setup GUI
window_name = "Vesselness Parameter GUI"
image = np.zeros((1, 512))
cv2.namedWindow(window_name)
cv2.createTrackbar("Hessian Side", window_name, 1, 4, do_nothing)
cv2.createTrackbar("Hessian Variance", window_name, 0, 255, do_nothing)
cv2.createTrackbar("Post Process Side", window_name, 1, 4, do_nothing)
cv2.createTrackbar("Post Process Variance", window_name, 0, 255, do_nothing)
cv2.createTrackbar("c Parameter", window_name, 0, 255, do_nothing)
cv2.createTrackbar("beta Parameter", window_name, 0, 255, do_nothing)
while not rospy.is_shutdown():
cv2.imshow(window_name, image)
k = cv2.waitKey(1) & 0xFF
if k in [27, ord('q')]:
rospy.signal_shutdown('Quit')
params.hessianSide = cv2.getTrackbarPos("Hessian Side", window_name)
params.hessianVariance = cv2.getTrackbarPos("Hessian Variance", window_name) / 10.0
params.postProcessSide = cv2.getTrackbarPos("Post Process Side", window_name)
params.postProcessVariance = cv2.getTrackbarPos("Post Process Variance", window_name) / 10.0
params.cParameter = 0.001 + 0.999 / 255 * cv2.getTrackbarPos("c Parameter", window_name)
params.betaParameter = 0.001 + 0.999 / 255 * cv2.getTrackbarPos("beta Parameter", window_name)
rospy.loginfo(params)
pub.publish(params)
rate.sleep()
def set_path(self, ogrid):
#group contam_points into boxes, prioritizing less dirty areas first
points = self.extract_points(ogrid)
if points.size==0:
rospy.signal_shutdown("Cleaning complete. Program will now exit.")
return
else:
n_clusters = int(round(points.size/2*(self.resolution**2))) #1 cluster ~= 1 sq. meter
print n_clusters
cluster_data = []
if n_clusters >= 1:
kmeans = KMeans(n_clusters).fit(points)
for n in xrange(n_clusters):
xy = points[kmeans.labels_==n]
#print "point {0}".format(n)
cluster_data.append(self.bounding_box(xy))
order = self._get_order([x[4] for x in cluster_data])
self.seq += 1
self.goals.extend(self._set_cluster_goals(points[kmeans.labels_==order[0][0]].tolist(), cluster_data[order[0][0]], order[0][1]))
# for o in order:
# self.goals.extend(self._set_cluster_goals(cluster_data[o[0]], o[1]))
# self.seq += 1
# elif n_clusters == 0 and -1 in db.labels_:
# xy = points[db.labels_== -1]
# for point in xy:
# header = Header(self.seq, rospy.Time.now(), '/map')
# self.seq += 1
# pose = PoseStamped(header, Pose(Point(point[0], point[1], 0), RIGHT))
# goal = MoveBaseGoal()
# goal.target_pose = pose
# self.goals.append(goal)
else:
rospy.signal_shutdown("Cleaning complete. Program will now exit.")
def callback(data):
""" This function is called for each published message
"""
message = data.data
print "I received the string", message
if message in 'xqs':
D.STOP = True
D.tank(0,0)
if message in 'xq':
time.sleep(.5) # wait a bit...
print "Goodbye!"
rospy.signal_shutdown("Quit requested.")
elif message == ' ':
D.tank(0,0)
elif message == 'F':
transition( 0.1, state_start )
elif message == 'R':
D.tank(200,200)
elif message == 'Q':
D.tank(-200,200)
elif message == 'S':
D.tank(200,-200)
elif message == 'T':
D.tank(-200,-200)
def talker(self):
# Main publisher logic- app branches out from here
self.pub = rospy.Publisher('hamr_command', HamrCommand, queue_size=10)
rospy.init_node('hamr_tester', anonymous=True)
rate = rospy.Rate(10) # 10hz
msg = HamrCommand()
while not rospy.is_shutdown():
drive_or_pid = raw_input("Drive, PID, Kill all Motors (kill), DD Set (DDS), Holonomic (holo), tests or Quit?\n")
drive_or_pid = str.strip(drive_or_pid.lower())
if drive_or_pid == 'drive':
self.drive_control(msg)
elif drive_or_pid == 'pid':
self.pid_control(msg)
self.get_val(msg)
elif drive_or_pid == 'kill':
print 'Killing all motors...'
self.kill_motors()
elif drive_or_pid == 'quit':
rospy.signal_shutdown('Node shut down')
elif drive_or_pid == 'dds':
self.dd_set(msg)
elif drive_or_pid == 'holo':
self.holonomic_control(msg)
elif drive_or_pid == 'test':
self.call_tests(msg)
else:
print "That's not a valid option"
rate.sleep()
def run(self):
move_thread = None
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
rospy.loginfo("Waiting for MoveIt...")
self.client = MoveGroupInterface("arm_with_torso", "base_link")
rospy.loginfo("...connected")
# Padding does not work (especially for self collisions)
# So we are adding a box above the base of the robot
scene = PlanningSceneInterface("base_link")
scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)
joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.client.moveToJointPosition(joints,
pose,
0.0,
max_velocity_scaling_factor=0.5)
if result and result.error_code.val == MoveItErrorCodes.SUCCESS:
scene.removeCollisionObject("keepout")
if move_thread:
move_thread.stop()
# On success quit
# Stopping the MoveIt thread works, however, the action client
# does not get shut down, and future tucks will not work.
# As a work-around, we die and roslaunch will respawn us.
rospy.signal_shutdown("done")
sys.exit(0)
return
def detect_people(self, img):
self.frame_width = img.width
cv_image = CvBridge().imgmsg_to_cv2(img, desired_encoding="bgr8")
print "what the fuk"
found, w = self.hog.detectMultiScale(cv_image, winStride=(8,8), padding=(32,32), scale=1.05)
found_filtered = []
for ri, r in enumerate(found):
for qi, q in enumerate(found):
if ri != qi and inside(r, q):
break
else:
found_filtered.append(r)
x, y, w, _ = found_filtered[0]
self.calculate_distance(x, w)
if self.cur_distance <= 1.0:
rospy.sleep(1)
rospy.signal_shutdown("Done.")
self.calculate_goal()
self.markov = mdp(self.goal)
self.pl = PolicyList()
self.pl.data = markov.policy_list
self.make_move()
def main():
global client
try:
rospy.init_node("test_move", anonymous=True, disable_signals=True)
client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
print "Waiting for server..."
client.wait_for_server()
print "Connected to server"
parameters = rospy.get_param(None)
index = str(parameters).find('prefix')
if (index > 0):
prefix = str(parameters)[index+len("prefix': '"):(index+len("prefix': '")+str(parameters)[index+len("prefix': '"):-1].find("'"))]
for i, name in enumerate(JOINT_NAMES):
JOINT_NAMES[i] = prefix + name
print "This program makes the robot move between the following three poses:"
print str([Q1[i]*180./pi for i in xrange(0,6)])
print str([Q2[i]*180./pi for i in xrange(0,6)])
print str([Q3[i]*180./pi for i in xrange(0,6)])
print "Please make sure that your robot can move freely between these poses before proceeding!"
inp = raw_input("Continue? y/n: ")[0]
if (inp == 'y'):
#move1()
move_repeated()
#move_disordered()
#move_interrupt()
else:
print "Halting program"
except KeyboardInterrupt:
rospy.signal_shutdown("KeyboardInterrupt")
raise
def spin(self):
self.diagnostic_pub = rospy.Publisher("/diagnostics", DiagnosticArray)
try:
reset_count = 0
rospy.loginfo("Camera synchronizer is running...")
controller_update_count = 0
while not rospy.is_shutdown():
if self.config['camera_reset'] == True:
reset_count = reset_count + 1
if reset_count > 3:
self.server.update_configuration({'camera_reset' : False})
else:
reset_count = 0
self.update_diagnostics()
# In case the controllers got restarted, refresh their state.
controller_update_count += 1
if controller_update_count >= 10:
controller_update_count = 0
for controller in self.controllers:
controller.update();
rospy.sleep(1)
finally:
rospy.signal_shutdown("Main thread exiting")
self.kill()
print "Main thread exiting"
def __init__(self):
#create our action server clients
self._left_client = actionlib.SimpleActionClient(
'robot/limb/left/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
self._right_client = actionlib.SimpleActionClient(
'robot/limb/right/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
#verify joint trajectory action servers are available
l_server_up = self._left_client.wait_for_server(rospy.Duration(10.0))
r_server_up = self._right_client.wait_for_server(rospy.Duration(10.0))
if not l_server_up or not r_server_up:
msg = ("Action server not available."
" Verify action server availability.")
rospy.logerr(msg)
rospy.signal_shutdown(msg)
sys.exit(1)
#create our goal request
self._l_goal = FollowJointTrajectoryGoal()
self._r_goal = FollowJointTrajectoryGoal()
#limb interface - current angles needed for start move
self._l_arm = baxter_interface.Limb('left')
self._r_arm = baxter_interface.Limb('right')
#gripper interface - for gripper command playback
self._l_gripper = baxter_interface.Gripper('left', CHECK_VERSION)
self._r_gripper = baxter_interface.Gripper('right', CHECK_VERSION)
#flag to signify the arm trajectories have begun executing
self._arm_trajectory_started = False
#reentrant lock to prevent same-thread lockout
self._lock = threading.RLock()
# Verify Grippers Have No Errors and are Calibrated
if self._l_gripper.error():
self._l_gripper.reset()
if self._r_gripper.error():
self._r_gripper.reset()
if (not self._l_gripper.calibrated()
and self._l_gripper.type() != 'custom'):
self._l_gripper.calibrate()
if (not self._r_gripper.calibrated()
and self._r_gripper.type() != 'custom'):
self._r_gripper.calibrate()
#gripper goal trajectories
self._l_grip = FollowJointTrajectoryGoal()
self._r_grip = FollowJointTrajectoryGoal()
# Timing offset to prevent gripper playback before trajectory has started
self._slow_move_offset = 0.0
self._trajectory_start_offset = rospy.Duration(0.0)
self._trajectory_actual_offset = rospy.Duration(0.0)
#param namespace
self._param_ns = '/rsdk_joint_trajectory_action_server/'
#gripper control rate
self._gripper_rate = 20.0 # Hz
#publish timing topics
self.timing_pub_state = rospy.Publisher('/board_pose/cycle_on',
std_msgs.Bool,
queue_size=0)
self.timing_msg_state = std_msgs.Bool()
self.timing_pub_time = rospy.Publisher('/board_pose/cycle_time',
std_msgs.Time,
queue_size=1)
self.timing_msg_time = std_msgs.Time()
def follow(self):
if not (self.target_found and self.object_location != None):
return
pid = self.camera_pid
pid.kp = -0.0005
pid.kd = 0.005
target_value = 0.0
current_value = self.object_location
print current_value
ang = pid.compute(current_value, target_value)
keep_distance = 0.3
spectrum = 20
mid = np.amin(np.concatenate((self.laser_values[-spectrum:], self.laser_values[:spectrum]), axis=0) )
#right = np.amin(self.laser_values[-2*spectrum:-spectrum])
#left = np.amin(self.laser_values[spectrum:spectrum*2])
#print 'left', left, 'mid', mid, 'right', right, ' ang', ang
if self.target_found:
if abs(current_value)<2:
distance = min(self.laser_values[0],3)
if distance < 0.1:
return
client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "base_link"
goal.target_pose.header.stamp = rospy.Time.now()
# Move 0.5 meters forward along the x axis of the "map" coordinate frame
goal.target_pose.pose.position.x = distance - 0.4
# No rotation of the mobile base frame w.r.t. map frame
goal.target_pose.pose.orientation.w = 1.0
print 'going on goal', goal
time.sleep(3.0)
SimpleActionClient = client.send_goal(goal)
time.sleep(2.0)
client.cancel_goal()
# Waits for the server to finish performing the action.
wait = client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
# Result of executing the action
print 'result', client.get_result()
br = tf.TransformBroadcaster()
br.sendTransform((distance, 0.0, 0.0),(0.0, 0.0, 0.0, 1.0),rospy.Time.now(),"/carrot1","/base_link")
ls = tf.TransformListener()
#print ls.allFramesAsString()
#print 'carrot', ls.frameExists("/carrot1"), 'map', ls.frameExists("map"), 'base_link', ls.frameExists("base_link")
if ls.frameExists("carrot1") and ls.frameExists("map"):
t = ls.getLatestCommonTime("carrot1", "map")
position, quaternion = ls.lookupTransform("carrot1", "map", t)
print 'pos', position
return
if mid<1.5:
#drive(mid - keep_distance ,ang)
vel = mid - keep_distance
else:
#drive(1,ang)
vel = 1
else:
#drive(0,0)
vel = 0
ang = 0
vel=0
self.drive(vel,-ang)
freeze = False
if state == 2:
freeze = False
if state == 1:
freeze = False
if state == 4 and state_transition_time_s > 30:
print("Shutting down because in state 4 for 30+ s")
#unix('sudo shutdown -h now')
if time_step.check():
print(
d2s("In state", state, "for", state_transition_time_s,
"seconds, previous_state =", previous_state))
time_step.reset()
if not folder_display_timer.check():
print("*** Data foldername = " + rp.foldername + '***')
if reload_timer.check():
reload(rp)
model_name_pub.publish(std_msgs.msg.String(rp.weights_file_path))
reload_timer.reset()
if git_pull_timer.check():
#unix(opjh('kzpy3/kzpy3_git_pull.sh'))
unix(opjh('model_car/model_car_git_pull.sh'))
git_pull_timer.reset()
except Exception as e:
print("********** Exception ***********************", 'red')
traceback.print_exc(file=sys.stdout)
rospy.signal_shutdown(d2s(e.message, e.args))
def main():
rospy.init_node('assembly_task_manager')
topic_name = '/assembly/state_transition'
planner = MoveGroupPlanner()
@smach.cb_interface(input_keys=['target_pose'])
def get_trajectory(ud, goal):
trajectory = planner.plan(ud.target_pose) # Good! Checked
return_goal = FollowJointTrajectoryGoal()
return_goal.trajectory = trajectory.joint_trajectory
#print(trajectory)
return return_goal
assembly_sm = StateMachine(outcomes=['finished', 'aborted', 'preempted'])
## predefined targets
# init position
joint_init_goal = [None] * 7
joint_init_goal[0] = 0
joint_init_goal[1] = -pi / 4
joint_init_goal[2] = 0
joint_init_goal[3] = -pi / 2
joint_init_goal[4] = 0
joint_init_goal[5] = pi / 3
joint_init_goal[6] = pi / 4
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.x = 0.9238795
pose_goal.orientation.y = -0.3826834
pose_goal.orientation.z = 0.
pose_goal.orientation.w = 0.
pose_goal.position.x = 0.4
pose_goal.position.y = 0.0
pose_goal.position.z = 0.12
assembly_sm.userdata.joint_init_goal = joint_init_goal
assembly_sm.userdata.pose1_goal = pose_goal
peg_goal1 = AssemblePegInHoleGoal()
peg_goal1.pitch = 0.0025
peg_goal1.linear_velocity = 0.005
peg_goal1.z_force = -6.0
peg_goal1.insert_force = -20.0
contact_force_goal = AssembleApproachGoal()
contact_force_goal.contact_force = 6
epsilon_inner = 0.002
epsilon_outer = 0.002
epsilon = franka_gripper.msg.GraspEpsilon(inner=epsilon_inner,
outer=epsilon_outer)
grasp_goal1 = franka_gripper.msg.GraspGoal(width=0.007,
epsilon=epsilon,
speed=0.1,
force=60)
gripper_open_goal = franka_gripper.msg.MoveGoal(width=0.1, speed=0.1)
## defining state machine structure
with assembly_sm:
StateMachine.add('READY',
StringTransitionState(topic_name,
outcomes=['initialize']),
transitions={'initialize': 'INITIALIZE1'})
StateMachine.add('INITIALIZE1',
SimpleActionState('/franka_gripper/move',
franka_gripper.msg.MoveAction,
goal=gripper_open_goal),
transitions={'succeeded': 'INITIALIZE2'})
StateMachine.add('INITIALIZE2',
SimpleActionState(
'/assembly_controller/joint_trajectory_control',
FollowJointTrajectoryAction,
goal_cb=get_trajectory),
transitions={'succeeded': 'READY_TO_MOVE'},
remapping={'target_pose': 'joint_init_goal'})
StateMachine.add('READY_TO_MOVE',
StringTransitionState(topic_name,
outcomes=['approach1']),
transitions={'approach1': 'APPROACH1'})
StateMachine.add('APPROACH1',
SimpleActionState(
'/assembly_controller/joint_trajectory_control',
FollowJointTrajectoryAction,
goal_cb=get_trajectory),
transitions={'succeeded': 'GRASP1'},
remapping={'target_pose': 'pose1_goal'})
StateMachine.add('GRASP1',
SimpleActionState('/franka_gripper/grasp',
franka_gripper.msg.GraspAction,
goal=grasp_goal1),
transitions={'succeeded': 'APPROACH_TO_COMPONENT'})
StateMachine.add('APPROACH_TO_COMPONENT',
SimpleActionState(
'/assembly_controller/assemble_approach_control',
AssembleApproachAction,
goal=contact_force_goal),
transitions={'succeeded': 'ASSEMBLE1'})
StateMachine.add('ASSEMBLE1',
SimpleActionState(
'/assembly_controller/assemble_peginhole_control',
AssemblePegInHoleAction,
goal=peg_goal1),
transitions={'succeeded': 'finished'})
#StateMachine.add('APPROACH1',
# SimpleActionState('/assembly_controller/joint_trajectory_control', JointControlAction, goal=joint_init_goal),
# transitions={'succeeded':'READY_TO_MOVE'})
# Run state machine introspection server
intro_server = smach_ros.IntrospectionServer('assembly', assembly_sm,
'/ASSEMBLY_1')
intro_server.start()
assembly_sm.execute()
rospy.spin()
intro_server.stop()
rospy.signal_shutdown('All done.')
while not rospy.is_shutdown():
msg.position[0:6] = ic.jointsPos
msg.position[6:9] = [0.0, 0.0, 0.0]
jointPub.publish(msg)
# for i in range(0, 5):
# x = raw_input ("Press to move arm")
# ic.moveArm(position_list[i])
# x = raw_input ("Press to get data")
# ic.jointReadList[i] = True
# x = raw_input ("Press to stop data")
# ic.jointReadList[i] = False
if (ic.finishedCalibrating):
for i in range(0,6):
print(len(ic.jointDataList[i]))
for i in range(0,6):
averages.append(np.average(ic.jointDataList[i]))
print(averages)
with open('imuRangePositions.csv', 'wb') as csvfile:
writer = csv.writer(csvfile, delimiter=',', quotechar='|', quoting=csv.QUOTE_MINIMAL)
writer.writerow(averages)
rospy.signal_shutdown("Done Calibrating")
rate.sleep()
def get_ctrl_output(self):
# get the location of the robot
try:
(translation, rotation) = self.trans_listener.lookupTransform(
"/map", "base_footprint", rospy.Time(0))
euler = tf.transformations.euler_from_quaternion(rotation)
self.x = translation[0]
self.y = translation[1]
self.th = euler[2]
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
translation = (0, 0, 0)
rotation = (0, 0, 0, 1)
euler = tf.transformations.euler_from_quaternion(rotation)
self.x = translation[0]
self.y = translation[1]
self.th = euler[2]
# Define Controller Gains
k1 = 0.5
k2 = 0.5
k3 = 0.5
# Distance to final point in current path
rho = np.linalg.norm(
np.asarray(self.x_goal[0:2]) - np.asarray([self.x, self.y]))
# Define relevant control parameters
alpha = wrapToPi(
np.arctan2(self.y_g - self.y, self.x_g - self.x) - self.th)
delta = wrapToPi(alpha + self.th - self.th_g)
#Define control inputs (V,om) - without saturation constraints
if self.Mode == 0: #'velocity_control'
rospy.loginfo("Velocity Mode")
V = self.V
om = self.om
else:
rospy.loginfo("Position Mode")
V = k1 * rho * np.cos(alpha)
om = k2 * alpha + k1 * np.sinc(
alpha / np.pi) * np.cos(alpha) * (alpha + k3 * delta)
# Check whether we're close to the goal, and override control inputs to force a stop
# at the target position
rospy.logwarn('Mission State is {}'.format(self.missionState))
if rho < self.STOP_THRESHOLD or self.missionState == self.MissionStates.END_OF_MISSION:
rospy.logwarn('Stopping Robot')
if self.missionState == self.MissionStates.END_OF_MISSION:
rospy.signal_shutdown(
'End of Mission. Shutting down controller.')
V = 0
om = 0
# Apply saturation limits
V = np.sign(V) * min(0.3, np.abs(V))
om = np.sign(om) * min(1.0, np.abs(om))
cmd_x_dot = V # forward velocity
cmd_theta_dot = om
cmd = Twist()
cmd.linear.x = cmd_x_dot
cmd.angular.z = cmd_theta_dot
return cmd
tf_listener = tf.TransformListener()
# parent frame for the listener
parent_frame = 'odom'
# child frame for the listener
child_frame = 'base_footprint'
# gains for the proportional controllers. These values can be tuned.
k_h_gain = 1
k_v_gain = 1
try:
tf_listener.waitForTransform(parent_frame, child_frame, rospy.Time(),
rospy.Duration(1.0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between {p} and {c}".format(
p=parent_frame, c=child_frame))
rospy.signal_shutdown("tf Exception")
# Class for storing node position, cost to come, parent index, and prev_orientation.
class Node:
def __init__(self, x, y, cost, parent_index, prev_orientation,
curr_orientation, UL, RL):
self.x = x
self.y = y
self.cost = cost
self.parent_index = parent_index
self.prev_orientation = prev_orientation
self.curr_orientation = curr_orientation
self.UL = UL
self.RL = RL
if __name__ == '__main__':
rospy.init_node('new_pose', argv=sys.argv, disable_signals=True)
listener = tf.TransformListener()
while True:
try:
translation, rotation = listener.lookupTransform('j2s7s300_link_base', 'ee_frame_camera_flipped', rospy.Time())
break # once the transform is obtained move on
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue # if it fails try again
pose = translation + rotation
print(pose)
updated_pose = rospy.ServiceProxy('new_pose', New_pose)
try:
answer = updated_pose(pose)
except rospy.ServiceException as e:
rospy.logwarn('Service call failed for: {0}'.format(e))
rospy.loginfo( '{0}'.format(answer.success))
rospy.signal_shutdown('{0}'.format(answer.success))
def __init__(self):
# params
self.dt = 0.01 # timestep of the simulation
t_final = rospy.get_param('/t_final')
do_liveplot = rospy.get_param('/do_liveplot')
save_logfile = rospy.get_param('/save_logfile')
# init node subs pubs
rospy.init_node('experiment_manager', anonymous=True)
#rospy.init_node('experiment_manager', anonymous=True, disable_signals=True)
self.clockpub = rospy.Publisher('/clock', Clock, queue_size=10)
self.pathlocalsub = rospy.Subscriber("pathlocal", Path,
self.pathlocal_callback)
self.obstaclesub = rospy.Subscriber("obstacles", Obstacles,
self.obstacles_callback)
self.trajhatsub = rospy.Subscriber("trajhat", Trajectory,
self.trajhat_callback)
self.trajstarsub = rospy.Subscriber("trajstar", Trajectory,
self.trajstar_callback)
self.statesub = rospy.Subscriber("state", State, self.state_callback)
self.dynamicparamsub = rospy.Subscriber("dynamic_vehicle_params",
DynamicVehicleParams,
self.dynamicparams_callback)
# load global path
self.loadPathGlobalFromFile()
# set static vehicle params
self.setStaticParams()
# init internal variables
self.counter = 0 # use this to reduce plot update rate
self.pathlocal = Path()
self.obstacles = Obstacles()
self.trajhat = Trajectory()
self.trajstar = Trajectory()
self.state = State()
self.dp = DynamicVehicleParams()
# init lists for logging
self.tvec = []
self.states = []
if (do_liveplot):
# init plot window
plt.ion()
self.f, (self.a0, self.a1) = plt.subplots(
1, 2, gridspec_kw={'width_ratios': [3, 1]})
self.a0.axis("equal")
self.a1.axis("equal")
# Main loop
self.t = 0
while (not rospy.is_shutdown()) and self.t < t_final:
# store data from subscribed topics
if (save_logfile):
self.stack_data()
# handle simtime
self.t += self.dt
msg = Clock()
t_rostime = rospy.Time(self.t)
msg.clock = t_rostime
self.clockpub.publish(msg)
if (do_liveplot):
self.liveplot()
slowdown_factor = 1
else:
slowdown_factor = 1
print 'simtime t =', t_rostime.to_sec()
time.sleep(self.dt * slowdown_factor)
print 'simulation finished'
if (save_logfile):
self.save_log()
# send shutdown signal
message = 'run finished, shutting down'
print message
rospy.signal_shutdown(message)
def __init__(self):
# Give the node a name
rospy.init_node('out_and_back', anonymous=False)
tf_prefix = rospy.get_param('tf_prefix', 'robot1')
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(self.shutdown)
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# How fast will we update the robot's movement?
rate = 200
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the forward linear speed to 0.2 meters per second
linear_speed = 20
# Set the travel points in meters
goal_x = [
0.0, 0.0, 42.0, 42.0, -12.0, -12.0, -32.0, -32.0, -8.0, -8.0, -8.0,
38.0, 38.0, 0.0, 0.0
]
goal_y = [
0.0, -2.0, -2.0, 52.0, 52.0, 18.0, 18.0, 22.0, 22.0, 18.0, 48.0,
48.0, 2.0, 2.0, 0.0
]
#goal_x = [ 0.0, 0.0, 4.0, 4.0, 5.0, 0.0, 0.0, -5.0,-4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
#goal_y = [ 0.0 , 1.0, 1.0, 2.0, 2.0, 2.0,-2.0, -2.0,-1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# Set the rotation speed in radians per second
angular_speed = 0.1
# Set the angular tolerance in degrees converted to radians
angular_tolerance = radians(5)
distance_tolerance = 0.01
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/' + tf_prefix + '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(
self.odom_frame, '/' + tf_prefix + '/base_footprint',
rospy.Time(), rospy.Duration(1))
self.base_frame = '/' + tf_prefix + '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(
self.odom_frame, '/' + tf_prefix + '/base_link',
rospy.Time(), rospy.Duration(1))
self.base_frame = '/' + tf_prefix + '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type
position = Point()
for i in range(len(goal_x)):
print("goal: x=%f, y=%f" % (goal_x[i], goal_y[i]))
move_cmd = Twist()
(position, rotation) = self.get_odom()
angle_to_goal = 0
while not_done(position.x, position.y, goal_x[i], goal_y[i]):
inc_x = goal_x[i] - position.x
inc_y = goal_y[i] - position.y
last_goal = angle_to_goal
angle_to_goal = atan2(inc_y, inc_x)
alpha = angle_to_goal - last_goal
if abs(alpha) > 1.5 * pi:
if alpha > 0:
angle_to_goal -= 2 * pi
else:
angle_to_goal += 2 * pi
# print("goal:")
# print(angle_to_goal)
# print(rotation)
if abs(angle_to_goal -
rotation) > abs(angular_tolerance) or abs(
angle_to_goal -
rotation) > pi - abs(angular_tolerance):
move_cmd.linear.x = 0
if angle_to_goal > rotation:
move_cmd.angular.z = 0.4
else:
move_cmd.angular.z = -0.4
else:
move_cmd.linear.x = 0.1
move_cmd.angular.z = 0.0
self.cmd_vel.publish(move_cmd)
r.sleep()
last_rotation = rotation
(position, rotation) = self.get_odom()
delta = rotation - last_rotation
if abs(delta) > pi:
if delta > 0:
rotation -= 2 * pi
else:
rotation += 2 * pi
# rotation = normalize_angle(rotation)
# Stop the robot before the next leg
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(2)
print("All done!")
os.system(
'cd $(rospack find omtb_map_extension_plugin)/scrips && sh save_map_2000.sh'
)
os.system(
'cd $(rospack find omtb_map_extension_plugin)/scrips && sh kill_bag.sh'
)
# Stop the robot for good
self.cmd_vel.publish(Twist())
def state_callback(msg):
global state
state = msg.data
if state == 7:
rospy.signal_shutdown('killed by selfdrive manager')
sys.exit()
#verify result
if all([l_finish, r_finish, l_result, r_result]):
return True
else:
msg = ("Trajectory action failed or did not finish before "
"timeout/interrupt.")
rospy.logwarn(msg)
return False
if __name__ == "__main__":
a = BaxterActuatorHandler(None, None, 'both', 100, 'position_w_id')
a.play_traj('lib/handlers/baxter/actions/pickup.rec',
'pickup',
1,
0,
initial=True)
a.play_traj('lib/handlers/baxter/actions/drop.rec',
'drop',
1,
0,
initial=True)
a.play_traj('lib/handlers/baxter/actions/pickup.rec',
'pickup',
1,
1,
initial=False)
rospy.signal_shutdown("Action completed.")
default='relaxedIK.config')
relaxedIK = RelaxedIK.init_from_config(config_file_name)
num_chains = relaxedIK.vars.robot.numChains
while eepg == None:
continue
rate = rospy.Rate(100)
while not rospy.is_shutdown():
pose_goals = eepg.ee_poses
header = eepg.header
num_poses = len(pose_goals)
if not num_poses == num_chains:
print bcolors.FAIL + 'ERROR: Number of pose goals ({}) ' \
'not equal to the number of kinematic chains ({}). Exiting relaxed_ik_node'.format(num_poses, num_chains)
rospy.signal_shutdown()
pos_goals = []
quat_goals = []
for p in pose_goals:
pos_x = p.position.x
pos_y = p.position.y
pos_z = p.position.z
quat_w = p.orientation.w
quat_x = p.orientation.x
quat_y = p.orientation.y
quat_z = p.orientation.z
pos_goals.append([pos_x, pos_y, pos_z])
self.targetSize = 1000
self.detection = False
self.controller.SetCoord(self.targetX, self.targetY, self.targetSize, self.detection)
finally:
self.imageLock.release()
def ReceiveNavdata(self,navdata):
# Update the message to be displayed
msg = self.StatusMessages[navdata.state] if navdata.state in self.StatusMessages else self.UnknownMessage
self.statusMessage = 'Connected:{}||{}||Battery:{}%||AutoTracking:{}'.format(self.connected,msg,int(navdata.batteryPercent),self.auto)
self.tagLock.acquire()
try:
if navdata.tags_count > 0:
self.tags = [(navdata.tags_xc[i],navdata.tags_yc[i],navdata.tags_distance[i]) for i in range(0,navdata.tags_count)]
else:
self.tags = []
finally:
self.tagLock.release()
if __name__=='__main__':
import sys
rospy.init_node('Tracking')
app = QtGui.QApplication(sys.argv)
#controller = BasicDroneController()
display = VideoProcessing()
display.show()
status = app.exec_()
rospy.signal_shutdown('Great Flying!')
sys.exit(status)
def callback(data):
rospy.on_shutdown(offhook)
global started
global max_speed
global max_angle
global lmt_speed
global lmt_angle
global cur_speed
global cur_angle
lefttrg = data.axes[2]
ritetrg = data.axes[5]
leftbump = data.buttons[4]
ritebump = data.buttons[5]
dpadx = data.axes[6]
dpady = data.axes[7]
leftanlgx = data.axes[0]
leftanlgy = data.axes[1]
start = data.buttons[0]
bigX = data.buttons[8]
if bigX == 1:
rospy.signal_shutdown("X pressed")
if start == 1:
started = not started
print ("ENABLED" if started else "DISABLED")
if started:
lmt_speed += dpady*2
lmt_angle += -1*dpadx*5
cur_angle = range_map(leftanlgx, 1, -1, -1*lmt_angle, lmt_angle)
if lefttrg == 1 or ritetrg == 1:
if lefttrg != 1:
cur_speed = range_map(lefttrg, -1, 1, -1*lmt_speed, 0)
elif ritetrg != 1:
cur_speed = range_map(ritetrg, 1, -1, 0, lmt_speed)
else:
cur_speed = 0
if lefttrg != 1 and ritetrg != 1:
cur_speed = 0
if leftbump == 1:
cur_angle = 0
if ritebump == 1:
cur_speed = 0
else:
cur_speed = 0
cur_angle = 0
msg = drive_param()
msg.velocity = cur_speed
msg.angle = cur_speed
print ('Limits: speed: %f, angle: %f' % (lmt_speed, lmt_angle))
print ('Current: speed: %f, angle: %f' % (cur_speed, cur_angle))
pub.publish(msg)
def main():
global client, MODE_DEBUG, MODE_REDUCE_FULL
try:
if (len(sys.argv) > 1):
if (sys.argv[1] == 'debug'):
MODE_DEBUG = True
print "MODE_DEBUG=" + str(MODE_DEBUG)
if (len(sys.argv) > 2):
if (sys.argv[2] == 'full'):
MODE_REDUCE_FULL = False
print "Mode Debug Full ? : " + str(MODE_REDUCE_FULL)
c_print("Initialisation du Publisher /ur_driver/URScript",
MODE_REDUCE_FULL)
pub = rospy.Publisher('/ur_driver/URScript', String, queue_size=10)
c_print("Initialisation du Publisher /wsg_50_driver/goal_position",
MODE_REDUCE_FULL)
pubGripper = rospy.Publisher('/wsg_50_driver/goal_position',
Cmd,
queue_size=10)
c_print(str("Initialisation du noeud :" + NODE_NAME), MODE_REDUCE_FULL)
rospy.init_node(NODE_NAME, anonymous=True, disable_signals=True)
print "This program makes the robot move, choose a mode:"
print " - Mode ROS (ROS FollowJointTrajectoryAction) : R"
print " - Mode URSCRIPT (movej, servoj) : U"
print " - Demo : D"
print " - Kinematics : K"
inp = raw_input("Mode? R/U/D/K: ")[0]
if (inp == 'R'):
client = actionlib.SimpleActionClient('follow_joint_trajectory',
FollowJointTrajectoryAction)
c_print("Waiting for server...", MODE_REDUCE_FULL)
client.wait_for_server()
c_print("Connected to server")
#move1()
move_repeated()
#move_disordered()
#move_interrupt()
elif (inp == 'U'):
c_print(str("Instanciation Kinematic UR10"), MODE_REDUCE_FULL)
kin = Kinematics('ur10')
c_print(str("Attente d'un message de JointState"),
MODE_REDUCE_FULL)
joint_states = rospy.wait_for_message("joint_states", JointState)
joints_pos = joint_states.position
c_print("JointState.position: " + str(tuple(joints_pos)),
MODE_REDUCE_FULL)
x = kin.forward(np.array(joints_pos))
c_print(str("Declaration point de depart du bras"), True)
c_print("First Pose " + str(x))
#Dessine un arc de cercle pour une translation de 10cm
test_incr([0.00, 0.30, 0.00], x, kin, pub, pubGripper, 300)
elif (inp == 'D'):
pubGripper.publish("open", 70.0, 4.0)
verif_move_gripper()
c_print(str("Instanciation Kinematic UR10"), MODE_REDUCE_FULL)
kin = Kinematics('ur10')
c_print(str("Attente d'un message de JointState"),
MODE_REDUCE_FULL)
joint_states = rospy.wait_for_message("joint_states", JointState)
joints_pos = joint_states.position
c_print("JointState.position: " + str(tuple(joints_pos)),
MODE_REDUCE_FULL)
x = kin.forward(np.array(joints_pos))
c_print(str("Placement pour Pion 1"), True)
x = [[0.00, -1.00, 0.00, 0.00], [0.00, 0.00, 1.00, 0.38],
[-1.00, 0.00, 0.00, 0.19], [0., 0., 0., 1.]]
c_print("First Pose " + str(x))
#print "Modif : ",x
sol = kin.inverse(x, joints_pos)
tmp = np.array2string(sol,
precision=8,
separator=',',
suppress_small=True)
send = "movej(" + tmp + ",t=5.0)"
c_print(send, MODE_REDUCE_FULL)
pub.publish(send)
verif_move(sol)
pubGripper.publish("close", 34.8, 4.0)
verif_move_gripper()
c_print(str("Deplacement pour Pion 1"), True)
move_diag_right_top(x)
sol = kin.inverse(x, joints_pos)
tmp = np.array2string(sol,
precision=8,
separator=',',
suppress_small=True)
send = "movej(" + tmp + ",t=5.0)"
c_print(send, MODE_REDUCE_FULL)
pub.publish(send)
verif_move(sol)
pubGripper.publish("open", 50.0, 4.0)
verif_move_gripper()
c_print(str("Position temporaire"), True)
move_leave(x)
sol = kin.inverse(x, joints_pos)
tmp = np.array2string(sol,
precision=8,
separator=',',
suppress_small=True)
send = "movej(" + tmp + ",t=5.0)"
c_print(send, MODE_REDUCE_FULL)
pub.publish(send)
verif_move(sol)
pubGripper.publish("open", 60.0, 4.0)
verif_move_gripper()
c_print(str("Placement pour Pion 2"), True)
move_left(x)
move_left(x)
move_top(x)
move_top(x)
move_take(x)
sol = kin.inverse(x, joints_pos)
tmp = np.array2string(sol,
precision=8,
separator=',',
suppress_small=True)
send = "movej(" + tmp + ",t=5.0)"
c_print(send, MODE_REDUCE_FULL)
pub.publish(send)
verif_move(sol)
pubGripper.publish("close", 34.8, 4.0)
verif_move_gripper()
c_print(str("Deplacement pour Pion 2"), True)
move_diag_right_down(x)
sol = kin.inverse(x, joints_pos)
tmp = np.array2string(sol,
precision=8,
separator=',',
suppress_small=True)
send = "movej(" + tmp + ",t=5.0)"
c_print(send, MODE_REDUCE_FULL)
pub.publish(send)
verif_move(sol)
pubGripper.publish("open", 50.0, 4.0)
verif_move_gripper()
c_print(str("Position temporaire"), True)
move_leave(x)
sol = kin.inverse(x, joints_pos)
tmp = np.array2string(sol,
precision=8,
separator=',',
suppress_small=True)
send = "movej(" + tmp + ",t=5.0)"
print send
pub.publish(send)
verif_move(sol)
c_print(str("Placement Pion 1 mange Pion2"), True)
move_diag_right_down(x)
move_take(x)
sol = kin.inverse(x, joints_pos)
tmp = np.array2string(sol,
precision=8,
separator=',',
suppress_small=True)
send = "movej(" + tmp + ",t=5.0)"
c_print(send, MODE_REDUCE_FULL)
pub.publish(send)
verif_move(sol)
pubGripper.publish("close", 34.8, 4.0)
verif_move_gripper()
c_print(str("Position temporaire"), True)
move_leave(x)
sol = kin.inverse(x, joints_pos)
tmp = np.array2string(sol,
precision=8,
separator=',',
suppress_small=True)
send = "movej(" + tmp + ",t=5.0)"
c_print(send, MODE_REDUCE_FULL)
pub.publish(send)
verif_move(sol)
c_print(str("Placement pour manger Pion 2"), True)
move_left(x)
move_left(x)
move_top(x)
move_top(x)
move_take(x)
sol = kin.inverse(x, joints_pos)
tmp = np.array2string(sol,
precision=8,
separator=',',
suppress_small=True)
send = "movej(" + tmp + ",t=5.0)"
c_print(send, MODE_REDUCE_FULL)
pub.publish(send)
verif_move(sol)
pubGripper.publish("open", 50.0, 4.0)
verif_move_gripper()
c_print(str("Position temporaire"), True)
move_leave(x)
sol = kin.inverse(x, joints_pos)
tmp = np.array2string(sol,
precision=8,
separator=',',
suppress_small=True)
send = "movej(" + tmp + ",t=5.0)"
c_print(send, MODE_REDUCE_FULL)
pub.publish(send)
verif_move(sol)
c_print(str("Pion qui meurt"), True)
move_diag_right_down(x)
move_take(x)
sol = kin.inverse(x, joints_pos)
tmp = np.array2string(sol,
precision=8,
separator=',',
suppress_small=True)
send = "movej(" + tmp + ",t=5.0)"
c_print(send, MODE_REDUCE_FULL)
pub.publish(send)
verif_move(sol)
pubGripper.publish("close", 34.8, 4.0)
verif_move_gripper()
c_print(str("Position temporaire"), True)
move_leave(x)
sol = kin.inverse(x, joints_pos)
tmp = np.array2string(sol,
precision=8,
separator=',',
suppress_small=True)
send = "movej(" + tmp + ",t=5.0)"
c_print(send, MODE_REDUCE_FULL)
pub.publish(send)
verif_move(sol)
c_print(str("Pion 2 mort"), True)
move_kill(x)
move_take(x)
sol = kin.inverse(x, joints_pos)
tmp = np.array2string(sol,
precision=8,
separator=',',
suppress_small=True)
send = "movej(" + tmp + ",t=5.0)"
c_print(send, MODE_REDUCE_FULL)
pub.publish(send)
verif_move(sol)
pubGripper.publish("open", 50.0, 4.0)
verif_move_gripper()
elif (inp == 'K'):
TestKin.main()
else:
print "Halting program"
except KeyboardInterrupt:
rospy.signal_shutdown("KeyboardInterrupt")
raise
def stop_ros(reason):
rospy.signal_shutdown(reason)
roscpp_shutdown()
cmds = cmd
results = []
for cmd in cmds:
result = self._execSingle(cmd, shell, silent)
results.append(result)
return results
else:
return self._execSingle(cmd, shell, silent)
def _execSingle(self, cmd, shell=False, silent=False):
if not silent:
rospy.loginfo('Executing command "%s"' % cmd)
if not shell:
cmd = cmd.split(' ')
p = subprocess.Popen(cmd, shell=shell, stdout=PIPE, stderr=PIPE)
(stdout, stderr) = p.communicate()
returncode = p.returncode
result = CmdResult(cmd, returncode, stdout, stderr)
if result.failed and not silent:
rospy.logerr('Command "%s" failed' % cmd)
rospy.logerr(str(result))
return result
if __name__ == '__main__':
rospy.init_node('squirrel_website_builder')
repository = rospy.get_param('~repository')
Builder(repository).build()
rospy.signal_shutdown('Build finished')
def __init__(self):
global p1, p2
self.ERRORS = {
0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
0x0001:
(diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
0x0002:
(diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
0x0004:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
0x0008:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Temperature2"),
0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Main batt voltage high"),
0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Logic batt voltage high"),
0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Logic batt voltage low"),
0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"M1 driver fault"),
0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"M2 driver fault"),
0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Main batt voltage high"),
0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Main batt voltage low"),
0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Temperature1"),
0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Temperature2"),
0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")
}
rospy.init_node("roboclaw_node_f")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
dev_name_front = rospy.get_param("~dev_front", "/dev/ttyACM0")
dev_name_back = rospy.get_param("~dev_back", "/dev/ttyACM1")
baud_rate = int(rospy.get_param("~baud", "38400"))
rospy.logwarn("after dev name")
self.address_front = int(rospy.get_param("~address_front", "128"))
self.address_back = int(rospy.get_param("~address_back", "129"))
#check wheather the addresses are in range
if self.address_front > 0x87 or self.address_front < 0x80 or self.address_back > 0x87 or self.address_back < 0x80:
rospy.logfatal("Address out of range")
rospy.signal_shutdown("Address out of range")
# TODO need someway to check if address is correct
try:
p1 = roboclaw.Open(dev_name_front, baud_rate)
except Exception as e:
rospy.logfatal("Could not connect to front Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to front Roboclaw")
try:
p2 = roboclaw.Open(dev_name_back, baud_rate)
except Exception as e:
rospy.logfatal("Could not connect to back Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to back Roboclaw")
#run diagnostics
# self.updater = diagnostic_updater.Updater() ###check later may be do it for both the motors
# self.updater.setHardwareID("Roboclaw")
# self.updater.add(diagnostic_updater.
# FunctionDiagnosticTask("Vitals", self.check_vitals))
#check if you can get the version of the roboclaw...mosly you dont
try:
version = roboclaw.ReadVersion(self.address_front, p1)
except Exception as e:
rospy.logwarn("Problem getting front roboclaw version")
rospy.logdebug(e)
pass
if not version[0]:
rospy.logwarn("Could not get version from front roboclaw")
else:
rospy.logdebug(repr(version[1]))
try:
version = roboclaw.ReadVersion(self.address_back, p2)
except Exception as e:
rospy.logwarn("Problem getting back roboclaw version")
rospy.logdebug(e)
pass
if not version[0]:
rospy.logwarn("Could not get version from back roboclaw")
else:
rospy.logdebug(repr(version[1]))
# roboclaw.SpeedM1M2(self.address_front, 0, 0,p1)
roboclaw.ResetEncoders(self.address_front, p1)
# roboclaw.SpeedM1M2(self.address_back, 0, 0,p2)
roboclaw.ResetEncoders(self.address_back, p2)
self.MAX_SPEED = float(rospy.get_param("~max_speed", "1.1"))
self.TICKS_PER_METER = float(
rospy.get_param("~ticks_per_meter", "35818"))
self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.1762"))
self.BASE_LENGTH = float(rospy.get_param("~base_length", "0.2485"))
self.WHEEL_RADIUS = float(rospy.get_param("~wheel_radius", "0.0635"))
self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH,
self.BASE_LENGTH, self.WHEEL_RADIUS)
self.last_set_speed_time = rospy.get_rostime()
rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback)
rospy.Subscriber("gains", Twist, self.gain_callback)
# rospy.sleep(1)
rospy.logdebug("dev_front %s dev_back %s", dev_name_front,
dev_name_back)
rospy.logdebug("baud %d", baud_rate)
rospy.logdebug("address_front %d address_back %d", self.address_front,
self.address_back)
rospy.logdebug("max_speed %f", self.MAX_SPEED)
rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER)
rospy.logdebug("base_width %f base_length %f", self.BASE_WIDTH,
self.BASE_LENGTH)
def __init__(self):
# Give the node a name
rospy.init_node('nav_square', anonymous=False)
# Set rospy to execute a shutdown function when terminating the script
rospy.on_shutdown(self.shutdown)
# How fast will we check the odometry values?
rate = 20
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the parameters for the target square
goal_distance = rospy.get_param("~goal_distance", 1.0) # meters
goal_angle = radians(rospy.get_param(
"~goal_angle", 90)) # degrees converted to radians
linear_speed = rospy.get_param("~linear_speed",
0.2) # meters per second
angular_speed = rospy.get_param("~angular_speed",
0.7) # radians per second
angular_tolerance = radians(rospy.get_param("~angular_tolerance",
2)) # degrees to radians
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
self.base_frame = rospy.get_param('~base_frame', '/base_link')
# The odom frame is usually just /odom
self.odom_frame = rospy.get_param('~odom_frame', '/odom')
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_footprint', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_link', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type
position = Point()
# Cycle through the four sides of the square
for i in range(4):
# Initialize the movement command
move_cmd = Twist()
# Set the movement command to forward motion
move_cmd.linear.x = linear_speed
# Get the starting position values
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
# Keep track of the distance traveled
distance = 0
# Enter the loop to move along a side
while distance < goal_distance and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current position
(position, rotation) = self.get_odom()
# Compute the Euclidean distance from the start
distance = sqrt(
pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))
# Stop the robot before rotating
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
# Set the movement command to a rotation
move_cmd.angular.z = angular_speed
# Track the last angle measured
last_angle = rotation
# Track how far we have turned
turn_angle = 0
# Begin the rotation
while abs(turn_angle + angular_tolerance) < abs(
goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last lopp
delta_angle = normalize_angle(rotation - last_angle)
turn_angle += delta_angle
last_angle = rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
# Stop the robot when we are done
self.cmd_vel.publish(Twist())
def __init__(self):
self.ERRORS = {0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"),
0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"),
0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"),
0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"),
0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"),
0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"),
0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"),
0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"),
0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"),
0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"),
0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")}
rospy.init_node("roboclaw_node", log_level=rospy.DEBUG) #TODO: remove 2nd param when done debugging
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
self.dev_name = rospy.get_param("~dev", "/dev/ttyACM0")
self.baud_rate = int(rospy.get_param("~baud", "115200"))
self.address = int(rospy.get_param("~address", "128"))
if self.address > 0x87 or self.address < 0x80:
rospy.logfatal("Address out of range")
rospy.signal_shutdown("Address out of range")
# TODO (Matt): figure out why roboclaw sometimes fails to Open
try:
roboclaw.Open(self.dev_name, self.baud_rate)
except Exception as e:
rospy.logfatal("Could not connect to Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to Roboclaw")
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(diagnostic_updater.
FunctionDiagnosticTask("Vitals", self.check_vitals))
try:
version = roboclaw.ReadVersion(self.address)
except Exception as e:
rospy.logwarn("Problem getting roboclaw version")
rospy.logdebug(e)
pass
if version is None:
rospy.logwarn("Could not get version from roboclaw")
else:
rospy.logdebug(repr(version[1]))
roboclaw.SpeedM1M2(self.address, 0, 0)
roboclaw.ResetEncoders(self.address)
self.LINEAR_MAX_SPEED = float(rospy.get_param("linear/x/max_velocity", "2.0"))
self.ANGULAR_MAX_SPEED = float(rospy.get_param("angular/z/max_velocity", "2.0"))
self.TICKS_PER_METER = float(rospy.get_param("tick_per_meter", "10"))
self.BASE_WIDTH = float(rospy.get_param("base_width", "0.315"))
self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
self.last_set_speed_time = rospy.get_rostime()
self.sub = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=5)
self.TIMEOUT = 2
rospy.sleep(1)
rospy.logdebug("dev %s", self.dev_name)
rospy.logdebug("baud %d", self.baud_rate)
rospy.logdebug("address %d", self.address)
rospy.logdebug("max_speed %f", self.LINEAR_MAX_SPEED)
rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER)
rospy.logdebug("base_width %f", self.BASE_WIDTH)
def map_joystick(joystick, limb):
"""
maps joystick input to gripper commands
@param joystick: an instance of a Joystick
"""
print("Getting robot state... ")
rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
init_state = rs.state()
gripper = None
original_deadzone = None
def clean_shutdown():
if gripper and original_deadzone:
gripper.set_dead_zone(original_deadzone)
print("Exiting example.")
try:
gripper = intera_interface.Gripper(limb + '_gripper')
except (ValueError, OSError) as e:
rospy.logerr("Could not detect an electric gripper attached to the robot.")
clean_shutdown()
return
rospy.on_shutdown(clean_shutdown)
# abbreviations
jhi = lambda s: joystick.stick_value(s) > 0
jlo = lambda s: joystick.stick_value(s) < 0
bdn = joystick.button_down
bup = joystick.button_up
def print_help(bindings_list):
print("Press Ctrl-C to quit.")
for bindings in bindings_list:
for (test, _cmd, doc) in bindings:
if callable(doc):
doc = doc()
print("%s: %s" % (str(test[1]), doc))
def offset_position(offset_pos):
cmd_pos = max(min(gripper.get_position() + offset_pos, gripper.MAX_POSITION), gripper.MIN_POSITION)
gripper.set_position(cmd_pos)
print("commanded position set to {0} m".format(cmd_pos))
def update_velocity(offset_vel):
cmd_speed = max(min(gripper.get_cmd_velocity() + offset_vel, gripper.MAX_VELOCITY), gripper.MIN_VELOCITY)
gripper.set_cmd_velocity(cmd_speed)
print("commanded velocity set to {0} m/s".format(cmd_speed))
# decrease position dead_zone
original_deadzone = gripper.get_dead_zone()
# WARNING: setting the deadzone below this can cause oscillations in
# the gripper position. However, setting the deadzone to this
# value is required to achieve the incremental commands in this example
gripper.set_dead_zone(0.001)
rospy.loginfo("Gripper deadzone set to {}".format(gripper.get_dead_zone()))
num_steps = 8.0
percent_delta = 1.0 / num_steps
position_increment = (gripper.MAX_POSITION - gripper.MIN_POSITION) * percent_delta
velocity_increment = (gripper.MAX_VELOCITY - gripper.MIN_VELOCITY) * percent_delta
bindings_list = []
bindings = (
#(test, command, description)
((bdn, ['btnLeft']), (gripper.reboot, []), "reboot"),
((bdn, ['btnUp']), (gripper.calibrate, []), "calibrate"),
((bdn, ['leftTrigger']), (gripper.close, []), "close"),
((bup, ['leftTrigger']), (gripper.open, []), "open (release)"),
((bdn, ['leftBumper']), (gripper.stop, []), "stop"),
((jlo, ['leftStickVert']), (offset_position, [-position_increment]),
"decrease position"),
((jhi, ['leftStickVert']), (offset_position, [position_increment]),
"increase position"),
((jlo, ['rightStickVert']), (update_velocity, [-velocity_increment]),
"decrease commanded velocity"),
((jhi, ['rightStickVert']), (update_velocity, [velocity_increment]),
"increase commanded velocity"),
((bdn, ['function1']), (print_help, [bindings_list]), "help"),
((bdn, ['function2']), (print_help, [bindings_list]), "help"),
)
bindings_list.append(bindings)
rospy.loginfo("Enabling robot...")
rs.enable()
rate = rospy.Rate(100)
print_help(bindings_list)
print("Press <Start> button for help; Ctrl-C to stop...")
while not rospy.is_shutdown():
# test each joystick condition and call binding cmd if true
for (test, cmd, doc) in bindings:
if test[0](*test[1]):
print(doc)
cmd[0](*cmd[1])
rate.sleep()
rospy.signal_shutdown("Example finished.")
rospy.sleep(10) ## TODO: Should times for .sleep be configurable??
rospy.loginfo("Reinitializing Node")
else:
## Everything is initialised and ready. Wait for requests:
try:
rospy.loginfo("...Image Adaptor initialized...")
except KeyboardInterrupt:
rospy.loginfo("Shutting down node.")
else:
rospy.spin()
return
###############################################################################
## ***** ***** ENTRY POINT FOR STARTING THE CODE ***** *****
###############################################################################
if __name__ == '__main__':
try:
basename = sys.argv[0].split('/')[-1]
if basename == "":
basename = sys.argv[0].split('/')[-2]
if basename[-3:] == ".py":
basename = basename[0:-3]
mainFunction(basename)
except Exception as e:
rospy.logerr("WHOLE-SCOPE EXCEPTION - NODE SHUTTING DOWN")
rospy.logerr(("Error: %s." % e).center(80, '*'))
finally:
rospy.signal_shutdown("testing shutdown")
if dynamixel_io.exception: raise dynamixel_io.exception
except dynamixel_io.FatalErrorCodeError, fece:
rospy.logerr(fece)
except dynamixel_io.NonfatalErrorCodeError, nfece:
self.error_counts['non_fatal'] += 1
rospy.logdebug(nfece)
except dynamixel_io.ChecksumError, cse:
self.error_counts['checksum'] += 1
rospy.logdebug(cse)
except dynamixel_io.DroppedPacketError, dpe:
self.error_counts['dropped'] += 1
rospy.logdebug(dpe.message)
except OSError, ose:
if ose.errno != errno.EAGAIN:
rospy.logfatal(errno.errorcode[ose.errno])
rospy.signal_shutdown(errno.errorcode[ose.errno])
if motor_states:
msl = MotorStateList()
msl.motor_states = motor_states
self.motor_states_pub.publish(msl)
self.current_state = msl
# calculate actual update rate
current_time = rospy.Time.now()
rates.append(1.0 / (current_time - last_time).to_sec())
self.actual_rate = round(sum(rates) / num_events, 2)
last_time = current_time
rate.sleep()
def run(self):
# ====== Setup section =====
counter = 0
counter_waypoint = 0
counter_datasave = 0
counter_total_datasaved = 0
timestamp = 0
# ====== Setup section =====
while not rospy.is_shutdown():
if self.init:
data_timestamp.append(timestamp)
data_temperature.append(self.currentTemperature)
data_salinity.append(self.currentSalinity)
data_x.append(self.vehicle_pos[0])
data_y.append(self.vehicle_pos[1])
data_z.append(self.vehicle_pos[-1])
data_lat.append(lat4)
data_lon.append(lon4)
if counter_datasave >= 10:
save_data(datapath, data_timestamp, data_lat, data_lon, data_x, data_y, data_z, data_salinity, data_temperature)
print("Data saved {:d} times".format(counter_total_datasaved))
counter_datasave = 0
counter_total_datasaved = counter_total_datasaved + 1
timestamp = timestamp + 1
counter_datasave = counter_datasave + 1
if self.auv_handler.getState() == "waiting":
if counter_waypoint == 0:
self.auv_handler.setWaypoint(deg2rad(lat_start), deg2rad(lon_start), depth_start)
else:
xcand, ycand, zcand = find_candidates_loc(xnow, ynow, znow, N1, N2, N3)
t1 = time.time()
xnext, ynext, znext = find_next_EIBV_1D(xcand, ycand, zcand,
xnow, ynow, znow,
xpre, ypre, zpre,
N1, N2, N3, Sigma_cond,
mu_cond, tau_sal, Threshold_S)
t2 = time.time()
t_elapsed.append(t2 - t1)
print("It takes {:.2f} seconds to compute the next waypoint".format(t2 - t1))
print("next is ", xnext, ynext, znext)
lat_next, lon_next = xy2latlon(xnext, ynext, origin, distance, alpha)
depth_next = depth_obs[znext]
ind_next = ravel_index([xnext, ynext, znext], N1, N2, N3)
F = np.zeros([1, N])
F[0, ind_next] = True
print("Arrived the starting location")
save_data(datapath, data_timestamp, data_lat, data_lon, data_x, data_y, data_z, data_salinity, data_temperature)
counter_total_datasaved = counter_total_datasaved + 1
print("Data saved {:02d} times".format(counter_total_datasaved))
print("Move to new way point, lat: {:.2f}, lon: {:.2f}, depth: {:.2f}".format(lat_next, lon_next, depth_next))
sal_sampled = np.mean(data_salinity[-10:]) # take the past ten samples and average
mu_cond, Sigma_cond = GPupd(mu_cond, Sigma_cond, R, F, sal_sampled)
xpre, ypre, zpre = xnow, ynow, znow
xnow, ynow, znow = xnext, ynext, znext
path.append([xnow, ynow, znow])
path_cand.append([xcand, ycand, zcand])
coords.append([lat_next, lon_next])
mu.append(mu_cond)
Sigma.append(Sigma_cond)
save_ESdata(datapath, path, path_cand, coords, mu, Sigma, t_elapsed)
# Move to the next waypoint
self.auv_handler.setWaypoint(deg2rad(lat_next), deg2rad(lon_next), depth_next)
counter_waypoint = counter_waypoint + 1
print("Waypoint is ", counter_waypoint)
if counter_waypoint >= N_steps:
rospy.signal_shutdown("Mission completed!!!")
rate = rospy.Rate(10)
while all((pub.get_num_connections() < num_subs,
rospy.Time.now() < time_end, not rospy.is_shutdown())):
rate.sleep()
return (pub.get_num_connections() >= num_subs)
if __name__ == '__main__':
rospy.init_node('motoman_test')
pub_traj = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=1)
if not WaitForSubscribers(pub_traj, 3.0):
print 'WARNING: No subscribers of /joint_path_command'
joint_names = [
'joint_' + jkey for jkey in ('s', 'l', 'e', 'u', 'r', 'b', 't')
]
joint_names = rospy.get_param('controller_joint_names')
t_traj = [4.0, 6.0, 8.0, 12.0]
q_traj = [[0.0] * 7, [0.1, -0.3, 0.15, -0.7, 0.1, -0.3, 0.15],
[0.21, -0.59, 0.30, -1.46, 0.35, -0.68, 0.31], [0.0] * 7]
dq_traj = [[0.0] * 7] * 4
traj = ToROSTrajectory(joint_names, q_traj, t_traj, dq_traj)
pub_traj.publish(traj)
rospy.signal_shutdown('Done.')
def create_window():
"""
Creates a GUI window to publish a pose.
"""
master = Tkinter.Tk()
height_min_box = Tkinter.Scale(master,
command=height_min,
from_=MIN_DISTANCE,
to=MAX_DISTANCE,
resolution=LINEAR_RESOLUTION,
label="Height (Minimum)")
height_min_box.grid(row=0, column=0)
height_max_box = Tkinter.Scale(master,
command=height_max,
from_=MIN_DISTANCE,
to=MAX_DISTANCE,
resolution=LINEAR_RESOLUTION,
label="Height (Maximum)")
height_max_box.grid(row=1, column=0)
zenith_min_box = Tkinter.Scale(master,
command=zenith_min,
from_=MIN_ORIENTATION,
to=MAX_ORIENTATION,
resolution=ANGULAR_RESOLUTION,
label="Zenith (Minimum)")
zenith_min_box.grid(row=0, column=1)
height_max_box = Tkinter.Scale(master,
command=zenith_max,
from_=MIN_ORIENTATION,
to=MAX_ORIENTATION,
resolution=ANGULAR_RESOLUTION,
label="Zenith (Maximum)")
height_max_box.grid(row=1, column=1)
azimuth_min_box = Tkinter.Scale(master,
command=azimuth_min,
from_=MIN_ORIENTATION,
to=MAX_ORIENTATION,
resolution=ANGULAR_RESOLUTION,
label="Azimuth (Minimum)")
azimuth_min_box.grid(row=0, column=2)
azimuth_max_box = Tkinter.Scale(master,
command=azimuth_max,
from_=MIN_ORIENTATION,
to=MAX_ORIENTATION,
resolution=ANGULAR_RESOLUTION,
label="Azimuth (Maximum)")
azimuth_max_box.grid(row=1, column=2)
yaw_min_box = Tkinter.Scale(master,
command=yaw_min,
from_=MIN_ORIENTATION,
to=MAX_ORIENTATION,
resolution=ANGULAR_RESOLUTION,
label="Yaw (Minimum)")
yaw_min_box.grid(row=0, column=3)
yaw_max_box = Tkinter.Scale(master,
command=yaw_max,
from_=MIN_ORIENTATION,
to=MAX_ORIENTATION,
resolution=ANGULAR_RESOLUTION,
label="Yaw (Maximum)")
yaw_max_box.grid(row=1, column=3)
radial_min_box = Tkinter.Scale(master,
command=radial_min,
from_=MIN_DISTANCE,
to=MAX_DISTANCE,
resolution=LINEAR_RESOLUTION,
label="Radial distance (Minimum)")
radial_min_box.grid(row=0, column=4)
radial_max_box = Tkinter.Scale(master,
command=radial_max,
from_=MIN_DISTANCE,
to=MAX_DISTANCE,
resolution=LINEAR_RESOLUTION,
label="Radial distance (Maximum)")
radial_max_box.grid(row=1, column=4)
master.title("Sampling parameters GUI")
master.mainloop()
rospy.signal_shutdown("GUI closed")
def __init__(self, port=None, baud=57600, timeout=5.0):
""" Initialize node, connect to bus, attempt to negotiate topics. """
self.mutex = thread.allocate_lock()
self.lastsync = rospy.Time(0)
self.lastsync_lost = rospy.Time(0)
self.timeout = timeout
self.synced = False
self.pub_diagnostics = rospy.Publisher(
'/diagnostics', diagnostic_msgs.msg.DiagnosticArray, queue_size=10)
if port == None:
# no port specified, listen for any new port?
pass
elif hasattr(port, 'read'):
#assume its a filelike object
self.port = port
else:
# open a specific port
try:
self.port = Serial(port, baud, timeout=self.timeout * 0.5)
except SerialException as e:
rospy.logerr("Error opening serial: %s", e)
rospy.signal_shutdown("Error opening serial: %s" % e)
raise SystemExit
self.port.timeout = 0.01 # Edit the port timeout
time.sleep(0.1) # Wait for ready (patch for Uno)
# hydro introduces protocol ver2 which must match node_handle.h
# The protocol version is sent as the 2nd sync byte emitted by each end
self.protocol_ver1 = '\xff'
self.protocol_ver2 = '\xfe'
self.protocol_ver = self.protocol_ver2
self.publishers = dict() # id:Publishers
self.subscribers = dict() # topic:Subscriber
self.services = dict() # topic:Service
self.buffer_out = -1
self.buffer_in = -1
self.callbacks = dict()
# endpoints for creating new pubs/subs
self.callbacks[TopicInfo.ID_PUBLISHER] = self.setupPublisher
self.callbacks[TopicInfo.ID_SUBSCRIBER] = self.setupSubscriber
# service client/servers have 2 creation endpoints (a publisher and a subscriber)
self.callbacks[
TopicInfo.ID_SERVICE_SERVER +
TopicInfo.ID_PUBLISHER] = self.setupServiceServerPublisher
self.callbacks[
TopicInfo.ID_SERVICE_SERVER +
TopicInfo.ID_SUBSCRIBER] = self.setupServiceServerSubscriber
self.callbacks[
TopicInfo.ID_SERVICE_CLIENT +
TopicInfo.ID_PUBLISHER] = self.setupServiceClientPublisher
self.callbacks[
TopicInfo.ID_SERVICE_CLIENT +
TopicInfo.ID_SUBSCRIBER] = self.setupServiceClientSubscriber
# custom endpoints
self.callbacks[
TopicInfo.ID_PARAMETER_REQUEST] = self.handleParameterRequest
self.callbacks[TopicInfo.ID_LOG] = self.handleLoggingRequest
self.callbacks[TopicInfo.ID_TIME] = self.handleTimeRequest
rospy.sleep(2.0) # TODO
self.requestTopics()
self.lastsync = rospy.Time.now()
signal.signal(signal.SIGINT, self.txStopRequest)
def temperature_process(temperature_data):
# turn ROS message into a numpy array
tempData = np.asarray(temperature_data.data)
# calibration routine
global calib_counter
global pix_calib
global deadpixel_map
calib_frames = 20
if calib_counter < calib_frames:
# for each pixel, get the average value over x frames
pix_calib = np.add(pix_calib, tempData)
print "Obtained Calibration image #", calib_counter + 1
calib_counter += 1
elif calib_counter == calib_frames:
pix_calib = pix_calib / (calib_frames)
meanpixel = np.mean(pix_calib)
for i in range(0, PIXEL_DATA_SIZE):
if (pix_calib[i] > meanpixel + 240) or (pix_calib[i] <
meanpixel - 240):
print("dead pixel at location {0} ({1} vs {2})".format(
i, pix_calib[i], meanpixel))
deadpixel_map = np.append(deadpixel_map, [i])
calib_counter += 1
print("dead pixels identified at locations:")
print np.size(deadpixel_map) - 1, ' dead pixels detected'
print "Calibration finished"
else:
# replace dead pixels with the pixel value of its neighboor (lazy, i know)
for i in range(1, len(deadpixel_map)):
tempData[deadpixel_map[i]] = tempData[deadpixel_map[i] - 1]
calib_gain = 2
# Temperature data into human viewable image
scaling_array = np.subtract(tempData, pix_calib) * calib_gain
temp_min = np.ndarray.min(scaling_array)
temp_max = np.ndarray.max(scaling_array)
# temp_min = -100
# temp_max = 100
temp_range = np.float(temp_max - temp_min)
floats = np.array((scaling_array - temp_min) / (temp_range),
dtype=float)
img = np.array(floats * 255, dtype=np.uint8)
# now put the pixels into a opencv image compatible numpy array
cvimg = np.zeros(PIXEL_DATA_SIZE, dtype=np.uint8)
for i in range(0, PIXEL_DATA_SIZE):
cvimg[(PIXEL_DATA_SIZE - ((i / 384) + 1) * 384 +
i % 384)] = np.uint8(floats[i] * 255)
cvimg = np.reshape(cvimg, (288, -1))
# display the image
cv2.imshow('cvimg', cvimg)
cv2.moveWindow('cvimg', 50, 50)
waitkey = cv2.waitKey(1)
if waitkey == 27 or waitkey == 113: # 'esc' and 'q' keys
print('exit key pressed')
rospy.signal_shutdown('exit key pressed')
elif waitkey == 99: # 'c' key
print "C key pressed - recalibrating. Keep the lens Covered."
calib_counter = 0
# Creates the SimpleActionClient, passing the type of the action
# (FibonacciAction) to the constructor.
client = actionlib.SimpleActionClient('/arm/joint_trajectory_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
# Waits until the action server has started up and started
# listening for goals.
client.wait_for_server(rospy.Duration(5.0))
# verify joint trajectory action servers are available
client_server = client.wait_for_server(rospy.Duration(10.0))
if not client_server:
msg = ("Action server not available."
" Verify action server availability.")
rospy.logerr(msg)
rospy.signal_shutdown(msg)
sys.exit(1)
# Creates a goal to send to the action server.
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = ['arm_elbow_joint',\
'arm_shoulder_lift_joint', 'arm_shoulder_pan_joint',\
'arm_wrist_1_joint', 'arm_wrist_2_joint', 'arm_wrist_3_joint']
goal.trajectory.points = []
point0 = JointTrajectoryPoint()
point0.positions = [0.0,0.0,0.0,0.0,0.0,0.0]
point0.velocities = [0.0,0.0,0.0,0.0,0.0,0.0]
# To be reached 1 second after starting along the trajectory
point0.time_from_start = rospy.Duration(1.0)
goal.trajectory.points.append(point0)