def __init__(self):
ButtonServer.global_server = self
self.handler = MyRequestHandler
rospy.init_node('button_server')
rospy.on_shutdown(self.on_shutdown)
self.pub = rospy.Publisher("buttons",std_msgs.msg.String,queue_size=1)
self.port = rospy.get_param("~port",5180)
self.blist = []
rospy.loginfo("Started Button Server on port %d" % self.port)
i=0
while True:
bname = rospy.get_param("~button%d"%i,"")
btext = rospy.get_param("~button%d_text"%i,bname)
bcolor = rospy.get_param("~button%d_color"%i,"")
bstyle = rospy.get_param("~button%d_style"%i,"")
if bname != "":
self.blist.append(Button(bname,btext,bcolor,bstyle))
else:
break
i = i+1
if len(self.blist)==0:
self.blist = [Button("test","Debug Button","lightgreen")]
self.buildPage()
self.root = roslib.packages.get_pkg_dir('button_server')
os.chdir(self.root)
self.httpd = MyServer(("", self.port), self.handler)
def main(name):
try:
from python_qt_binding.QtGui import QApplication
except:
print >> sys.stderr, "please install 'python_qt_binding' package!!"
sys.exit(-1)
masteruri = init_cfg_path()
parser = init_arg_parser()
args = rospy.myargv(argv=sys.argv)
parsed_args = parser.parse_args(args[1:])
# Initialize Qt
global app
app = QApplication(sys.argv)
# decide to show main or echo dialog
global main_form
if parsed_args.echo:
main_form = init_echo_dialog(name, masteruri, parsed_args.echo[0], parsed_args.echo[1], parsed_args.hz)
else:
main_form = init_main_window(name, masteruri, parsed_args.file)
# resize and show the qt window
if not rospy.is_shutdown():
os.chdir(PACKAGE_DIR) # change path to be able to the images of descriptions
main_form.resize(1024, 720)
screen_size = QApplication.desktop().availableGeometry()
if main_form.size().width() >= screen_size.width() or main_form.size().height() >= screen_size.height()-24:
main_form.showMaximized()
else:
main_form.show()
exit_code = -1
rospy.on_shutdown(finish)
exit_code = app.exec_()
def __init__(self):
rospy.init_node('Gocrazy', anonymous=False)
self.distance = 0
self.angle = 0
rospy.loginfo("To stop TurtleBot CTRL + C")
# What function to call when you ctrl + c
rospy.on_shutdown(self.shutdown)
# Create a publisher which can "talk" to TurtleBot and tell it to move
# Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if
# you're not using TurtleBot2
self.cmd_vel = rospy.Publisher(
'cmd_vel_mux/input/navi', Twist, queue_size=10)
# TurtleBot will stop if we don't keep telling it to move. How often
# should we tell it to move? 10 HZ
r = rospy.Rate(10)
move_cmd = Twist()
move_cmd.linear.x = self.distance
move_cmd.angular.y = self.angle
while not rospy.is_shutdown():
# publish the velocity
self.cmd_vel.publish(move_cmd)
# wait for 0.1 seconds (10 HZ) and publish again
r.sleep()
def __init__(self):
self.name = rospy.get_name()[1:]
self.action_list = ActionList(self.name)
self.action_list.update()
self.collection_list = CollectionList(self.name)
self.collection_list.update()
self.action_sequence_queue = []
self.action = None
# Reference to the action client or preview publisher.
self.execute_steps_relay = None
step_action_server_topic = rospy.get_param('/free_gait/action_server')
self.execute_steps_client = actionlib.SimpleActionClient(step_action_server_topic, free_gait_msgs.msg.ExecuteStepsAction)
self.execute_action_server = actionlib.SimpleActionServer("~execute_action", free_gait_msgs.msg.ExecuteActionAction, \
execute_cb=self._execute_action_callback, auto_start = False)
self.execute_action_server.register_preempt_callback(self.preempt)
self.execute_action_server.start()
step_preview_topic = rospy.get_param('/free_gait/preview_topic')
self.preview_publisher = rospy.Publisher(step_preview_topic, free_gait_msgs.msg.ExecuteStepsActionGoal, queue_size=1)
rospy.Service('~update', std_srvs.srv.Trigger, self.update)
rospy.Service('~list_actions', free_gait_msgs.srv.GetActions, self.list_actions)
rospy.Service('~list_collections', free_gait_msgs.srv.GetCollections, self.list_collections)
rospy.Service('~send_action', free_gait_msgs.srv.SendAction, self._send_action_callback)
rospy.Service('~preview_action', free_gait_msgs.srv.SendAction, self._preview_action_callback)
rospy.Service('~send_action_sequence', free_gait_msgs.srv.SendActionSequence, self._send_action_sequence_callback)
rospy.on_shutdown(self.preempt)
def __init__(self, name) :
rospy.on_shutdown(self._on_node_shutdown)
self.current_node = "Unknown"
self.nogo_pre_active = False
self.nogo_active = False
self.nogos=[]
self.stop_services=["/enable_motors", "/emergency_stop", "/task_executor/set_execution_status"]
self.activate_services=["/enable_motors", "/reset_motorstop", "/task_executor/set_execution_status"]
#Waiting for Topological Map
self._map_received=False
rospy.Subscriber('/topological_map', TopologicalMap, self.MapCallback)
rospy.loginfo("Waiting for Topological map ...")
while not self._map_received :
pass
rospy.loginfo(" ...done")
self.update_service_list()
print self.av_stop_services, self.av_activate_services
#Subscribing to Localisation Topics
rospy.loginfo("Subscribing to Localisation Topics")
rospy.Subscriber('/current_node', String, self.currentNodeCallback)
rospy.loginfo(" ...done")
self.nogos = self.get_no_go_nodes()
print self.nogos
self._killall_timers=False
t = Timer(1.0, self.timer_callback)
t.start()
rospy.loginfo("All Done ...")
rospy.spin()
def __init__(self):
"""
JARVIS User Interface Node
"""
rospy.on_shutdown(self.cleanup)
rospy.wait_for_service('return_grips')
self.msg = GoalID()
self.newGrips = jarvis_perception.msg.GraspArray()
self.legalUtterances = ['upper','lower','top','bottom','back','front','near','far','rear','left','right','anywhere','center']
self.lastUtterance = ''
self.axisAlignedBox = ''
self.person_trans = ''
self.person_rot = ''
plotting = True
self.likelihood = None
self.var = None
try:
grip_server = rospy.ServiceProxy('return_grips',ReturnGrips)
self.grips = grip_server()
print self.grips
pubgrips = rospy.Publisher('return_grips', jarvis_perception.msg.GraspArray, queue_size=10)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def __init__(self):
# set up openrave
self.env = rave.Environment()
self.env.StopSimulation()
self.env.Load("robots/pr2-beta-static.zae") # todo: use up-to-date urdf
self.robot = self.env.GetRobots()[0]
self.joint_listener = TopicListener("/joint_states", sm.JointState)
# rave to ros conversions
joint_msg = self.get_last_joint_message()
ros_names = joint_msg.name
inds_ros2rave = np.array([self.robot.GetJointIndex(name) for name in ros_names])
self.good_ros_inds = np.flatnonzero(inds_ros2rave != -1) # ros joints inds with matching rave joint
self.rave_inds = inds_ros2rave[self.good_ros_inds] # openrave indices corresponding to those joints
self.update_rave()
self.larm = Arm(self, "l")
self.rarm = Arm(self, "r")
self.lgrip = Gripper(self, "l")
self.rgrip = Gripper(self, "r")
self.head = Head(self)
self.torso = Torso(self)
self.base = Base(self)
rospy.on_shutdown(self.stop_all)
def __init__(self):
#start node
rospy.init_node("map_saver")
rospy.on_shutdown(self.die)
#get parameters from server
self.save_dir = rospy.get_param("map_save_dir","map_log/") #default to .ros/map_log
update_rate = rospy.get_param("map_save_update_rate",120) #update once every 2 min
#create directory and set clean_open
self.clean_open=self.create_dir()
#create dictionaries
self.edges={} #dictionary of edge_id:edge_obj
self.nodes={} #dictionary of node_id:node_obj
#begin listening
rospy.Subscriber("/node",GVGNode, self.node_handler)
rospy.Subscriber("/edge",GVGEdgeMsg, self.edge_handler)
self.updated=False
#while alive and able to open the file
self.alive=True
while(self.clean_open and self.alive):
if(self.updated):
self.dump(str(rospy.get_time()))
self.updated=False
rospy.sleep(update_rate)
def __init__(self):
'''
Creates a new instance. Find the topic of the master_discovery node using
U{master_discovery_fkie.interface_finder.get_changes_topic()
<http://docs.ros.org/api/master_discovery_fkie/html/modules.html#interface-finder-module>}.
Also the parameter C{~ignore_hosts} will be analyzed to exclude hosts from sync.
'''
self.masters = {}
# the connection to the local service master
self.materuri = masteruri_from_master()
'''@ivar: the ROS master URI of the C{local} ROS master. '''
self.__lock = threading.RLock()
# load interface
self._load_interface()
# subscribe to changes notifier topics
topic_names = interface_finder.get_changes_topic(masteruri_from_master())
self.sub_changes = dict()
'''@ivar: `dict` with topics {name: U{rospy.Subscriber<http://docs.ros.org/api/rospy/html/rospy.topics.Subscriber-class.html>}} publishes the changes of the discovered ROS masters.'''
for topic_name in topic_names:
rospy.loginfo("listen for updates on %s", topic_name)
self.sub_changes[topic_name] = rospy.Subscriber(topic_name, MasterState, self._rosmsg_callback_master_state)
self.__timestamp_local = None
self.__own_state = None
self.update_timer = None
self.own_state_getter = None
self._join_threads = dict() # threads waiting for stopping the sync thread
# initialize the ROS services
rospy.Service('~get_sync_info', GetSyncInfo, self._rosservice_get_sync_info)
rospy.on_shutdown(self.finish)
self.obtain_masters()
def _test_smach_execute2():
rospy.init_node('smach')
rospy.on_shutdown(myhook)
sq = Sequence(
outcomes = ['succeeded','aborted','preempted'],
connector_outcome = 'succeeded')
with sq:
Sequence.add('CHECK', CheckSmachEnabledState(),
transitions={'aborted':'SLEEP'})
Sequence.add('SLEEP', SleepState(10))
rospy.loginfo("Executing test...")
TransformListenerSingleton.get()
#outcome = sq.execute()
#rospy.loginfo("outcome: %s" % outcome)
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('server_name', sq, '/SM_ROOT')
sis.start()
# Execute the state machine
print "EXECUTING.."
outcome = sq.execute()
# Wait for ctrl-c to stop the application
print "SPINNING.ms."
rospy.spin()
#rospy.signal_shutdown("shutting down now")
sis.stop()
def __init__(self):
# Start node
rospy.init_node("recognizer_en")
self._device_name_param = "~mic_name" # Find the name of your microphone by typing pacmd list-sources in the terminal
self._lm_param = "~lm"
self._dic_param = "~dict"
# Configure mics with gstreamer launch config
if rospy.has_param(self._device_name_param):
self.device_name = rospy.get_param(self._device_name_param)
self.device_index = self.pulse_index_from_name(self.device_name)
self.launch_config = "pulsesrc device=" + str(self.device_index)
rospy.loginfo("Using: pulsesrc device=%s name=%s", self.device_index, self.device_name)
else:
self.launch_config = 'gconfaudiosrc'
rospy.loginfo("Launch config: %s", self.launch_config)
self.launch_config += " ! audioconvert ! audioresample " \
+ '! vader name=vad auto-threshold=true ' \
+ '! pocketsphinx name=asr ! fakesink'
# + '! pocketsphinx hmm=en_US/hub4wsj_sc_8k name=asr ! fakesink'
# Configure ROS settings
self.started = False
rospy.on_shutdown(self.shutdown)
self.pub = rospy.Publisher('~output', String)
rospy.Service("~start", Empty, self.start)
rospy.Service("~stop", Empty, self.stop)
if rospy.has_param(self._lm_param) and rospy.has_param(self._dic_param):
self.start_recognizer()
else:
rospy.logwarn("lm dic and hmm parameters need to be set to start recognizer.")
def __init__(self):
rospy.init_node('nav_test', anonymous=False)
#what to do if shut down (e.g. ctrl + C or failure)
rospy.on_shutdown(self.shutdown)
#tell the action client that we want to spin a thread by default
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("wait for the action server to come up")
#allow up to 5 seconds for the action server to come up
self.move_base.wait_for_server(rospy.Duration(5))
#we'll send a goal to the robot to move 3 meters forward
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'base_link'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 3.0 #3 meters
goal.target_pose.pose.orientation.w = 1.0 #go forward
#start moving
self.move_base.send_goal(goal)
#allow TurtleBot up to 60 seconds to complete task
success = self.move_base.wait_for_result(rospy.Duration(60))
if not success:
self.move_base.cancel_goal()
rospy.loginfo("The base failed to move forward 3 meters for some reason")
else:
# We made it!
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Hooray, the base moved 3 meters forward")
def main():
rospy.init_node("baxter_velocity_control")
# # FK Position
# print '\n*** Baxter Position FK ***\n'
# print kin.forward_position_kinematics()
# camera = Camera()
wobbler = Wobbler()
# for i in range(300):
# print i
# J = kin.jacobian_pseudo_inverse()
# A = np.matrix(J)
# twist = np.array([[.0],[.0],[.0],[.0],[.0],[.0]])
# q_dot = A*twist
# q_dot_list = [i[0] for i in q_dot.tolist()]
# # print q_dot_list
# wobbler.q_dot = q_dot_list
# wobbler.wobble()
wobbler.q_dot = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
wobbler.wobble()
# rospy.spin()
rospy.on_shutdown(wobbler.clean_shutdown)
print ("Done.")
def __init__(self):
rospy.on_shutdown(self.cleanup)
self.voice = rospy.get_param("~voice", "voice_kal_diphone")#voice_don_diphone, voice_kal_diphone
self.wavepath = rospy.get_param("~wavepath", "")
self.command_to_phrase = COMMAND_TO_PHRASE # this is deffined in wheelchairint/keywords_tocommand.py
# Create the sound client object
self.soundhandle = SoundClient()
# Announce that we are ready for input
rospy.sleep(2)
self.soundhandle.stopAll()
rospy.sleep(1)
self.soundhandle.playWave(self.wavepath + "/R2D2.wav")
rospy.sleep(2)
self.soundhandle.say("I am ready", self.voice)
rospy.sleep(1.5)
self.soundhandle.say("Give me an order", self.voice)
rospy.sleep(2)
rospy.loginfo("Say one a commands...")
# Subscribe to the recognizer output
rospy.Subscriber('recognizer/output', String, self.rec_out_callback)
r = rospy.Rate(0.5)
while not rospy.is_shutdown():
# rospy.loginfo("wheelchair talk is running correctly.")
r.sleep()
def __init__(self, name):
self.name = name
self.getConfig()
self.mission_status = MissionStatus()
# Init internal state
self.init_trajectory = False
self.init_keep_pose = False
self.init_goto = False
# Create "absolute" client
self.client_absolute = actionlib.SimpleActionClient('absolute_movement', WorldWaypointReqAction)
self.client_absolute.wait_for_server()
# Create publisher
self.pub_mission_status = rospy.Publisher("/safety_g500/mission_status", MissionStatus)
rospy.Timer(rospy.Duration(1.0), self.pubMissionStatus)
# Create Subscriber
rospy.Subscriber("/navigation_g500/nav_sts", NavSts, self.updateNavSts)
self.nav = NavSts()
# Create services
self.enable_trajectory = rospy.Service('/control_g500/enable_trajectory', Empty, self.enableTrajectory)
self.disable_trajectory = rospy.Service('/control_g500/disable_trajectory', Empty, self.disableTrajectory)
self.enable_keep_position = rospy.Service('/control_g500/enable_keep_position', Empty, self.enableKeepPosition)
self.disable_keep_position = rospy.Service('/control_g500/disable_keep_position', Empty, self.disableKeepPosition)
self.goto = rospy.Service('/control_g500/goto', GotoSrv, self.goto)
# Map shutdown function
rospy.on_shutdown(self.stop)
def __init__(self):
rospy.init_node('monitor_fake_battery', anonymous=False)
rospy.on_shutdown(self.shutdown)
# Set the low battery threshold (between 0 and 100)
self.low_battery_threshold = rospy.get_param('~low_battery_threshold', 50)
# Initialize the state machine
sm_battery_monitor = StateMachine(outcomes=[])
with sm_battery_monitor:
# Add a MonitorState to subscribe to the battery level topic
StateMachine.add('MONITOR_BATTERY',
MonitorState('battery_level', Float32, self.battery_cb),
transitions={'invalid':'RECHARGE_BATTERY',
'valid':'MONITOR_BATTERY',
'preempted':'MONITOR_BATTERY'},)
# Add a ServiceState to simulate a recharge using the set_battery_level service
StateMachine.add('RECHARGE_BATTERY',
ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, request=100),
transitions={'succeeded':'MONITOR_BATTERY',
'aborted':'MONITOR_BATTERY',
'preempted':'MONITOR_BATTERY'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('monitor_battery', sm_battery_monitor, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm_battery_monitor.execute()
intro_server.stop()
def __init__(self):
# initiliaze
rospy.init_node('GoForward', anonymous=False)
# tell user how to stop TurtleBot
rospy.loginfo("To stop TurtleBot CTRL + C")
# What function to call when you ctrl + c
rospy.on_shutdown(self.shutdown)
# Create a publisher which can "talk" to TurtleBot and tell it to move
# Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if you're not using TurtleBot2
self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
#TurtleBot will stop if we don't keep telling it to move. How often should we tell it to move? 10 HZ
r = rospy.Rate(10);
# Twist is a datatype for velocity
move_cmd = Twist()
# let's go forward at 0.2 m/s
move_cmd.linear.x = 0.3
# let's turn at 0 radians/s
move_cmd.angular.z = 0
# as long as you haven't ctrl + c keeping doing...
while not rospy.is_shutdown():
# publish the velocity
self.cmd_vel.publish(move_cmd)
# wait for 0.1 seconds (10 HZ) and publish again
r.sleep()
def __init__(self):
rospy.init_node("baxter_hydra_teleop")
self.status_display = baxter_faces.FaceImage()
rospy.loginfo("Getting robot state... ")
self.rs = baxter_interface.RobotEnable()
self.hydra_msg = Hydra()
self.hydra_msg_lock = threading.Lock()
self.gripper_left = baxter_interface.Gripper("left")
self.gripper_right = baxter_interface.Gripper("right")
self.mover_left = LimbMover("left")
self.mover_right = LimbMover("right")
self.mover_head = HeadMover()
self.happy_count = 0 # Need inertia on how long unhappy is displayed
self.hydra_msg = Hydra()
rospy.on_shutdown(self._cleanup)
rospy.Subscriber("/hydra_calib", Hydra, self._hydra_cb)
rospy.Timer(rospy.Duration(1.0 / 30), self._main_loop)
rospy.loginfo(
"Press left or right button on Hydra to start the teleop")
while not self.rs.state().enabled and not rospy.is_shutdown():
rospy.Rate(10).sleep()
self.mover_left.enable()
self.mover_right.enable()
self.mover_head.set_pose()
def __init__(self):
self.node_name = "face_recog_eigen"
rospy.init_node(self.node_name)
rospy.on_shutdown(self.cleanup)
self.bridge = CvBridge()
self.face_names = StringArray()
self.size = 4
face_haar = 'haarcascade_frontalface_default.xml'
self.haar_cascade = cv2.CascadeClassifier(face_haar)
self.face_dir = 'face_data_eigen'
# self.model = cv2.createFisherFaceRecognizer()
self.model = cv2.createEigenFaceRecognizer()
(self.im_width, self.im_height) = (112, 92)
rospy.loginfo("Loading data...")
# self.fisher_train_data()
self.load_trained_data()
rospy.sleep(3)
# self.img_sub = rospy.Subscriber("/asus/rgb/image_raw", Image, self.img_callback)
self.img_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.img_callback)
# self.img_pub = rospy.Publisher('face_img', Image, queue_size=10)
self.name_pub = rospy.Publisher('face_names', StringArray, queue_size=10)
self.all_names_pub = rospy.Publisher('all_face_names', StringArray, queue_size=10)
rospy.loginfo("Detecting faces...")
def __init__(self):
self.node_name = rospy.get_name()
self.br = tf.TransformBroadcaster()
self.CPR = 70 # encoder pusle per round 2*75
self.radius = 0.0215 # radius of wheel in meter
self.width = 0.22 # width of robot in meter
self.x = 0 # robot position x in meter
self.y = 0 # robot position y in meter
self.theta = 0 # robot pose theta in radian
self.encoder_pos_L = 0 # post left wheel encoder
self.encoder_pos_R = 0 # post right wheel encoder
self.encoder_pre_L = 0 # present left wheel encoder
self.encoder_pre_R = 0 # present right wheel encoder
self.R = 0
self.P = 0
# setup pi GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setup(22, GPIO.IN)
GPIO.setup(23, GPIO.IN)
GPIO.setup(24, GPIO.IN)
GPIO.setup(27, GPIO.IN)
# interrupt callback functions
GPIO.add_event_detect(23, GPIO.RISING, callback = self.Encoder_L)
GPIO.add_event_detect(27, GPIO.RISING, callback = self.Encoder_R)
# subscriber
self.sub_RP = rospy.Subscriber("~orientationRP", Float64MultiArray, self.cbOrientation)
rospy.on_shutdown(self.custom_shutdown) # shutdown method
self.update_timer = rospy.Timer(rospy.Duration.from_sec(1), self.update) # timer for update robot pose, update rate = 1 Hz
rospy.loginfo("[%s] Initialized " %self.node_name)
def __init__(self):
rospy.loginfo("Starting valve task controller...")
rospy.on_shutdown(self.cleanup)
# Initialize the state machine
self.running = False
self.safed = False
self.sm = smach.StateMachine(outcomes=['DONE', 'FAILED', 'SAFE'], input_keys=['sm_input'], output_keys=['sm_output'])
# Populate the state machine from the modules
with self.sm:
self.safemode = SafeMode.SAFEMODE()
# Add start state
smach.StateMachine.add('StartTask', StartTask.STARTTASK(), transitions={'Start':'FindValve'}, remapping={'input':'sm_input', 'output':'sm_data'})
# Add finding step
smach.StateMachine.add('FindValve', FindValve.FINDVALVE(), transitions={'Success':'PositionRobot', 'Failure':'ErrorHandler', 'Fatal':'SafeMode'}, remapping={'input':'sm_data', 'output':'sm_data'})
# Add positioning step
smach.StateMachine.add('PositionRobot', PositionRobot.POSITIONROBOT(), transitions={'Success':'PlanTurning', 'Failure':'ErrorHandler', 'Fatal':'SafeMode'}, remapping={'input':'sm_data', 'output':'sm_data'})
# Add planning step
smach.StateMachine.add('PlanTurning', PlanTurning.PLANTURNING(), transitions={'Success':'ExecuteTurning', 'Failure':'ErrorHandler', 'Fatal':'SafeMode'}, remapping={'input':'sm_data', 'output':'sm_data'})
# Add execution step
smach.StateMachine.add('ExecuteTurning', ExecuteTurning.EXECUTETURNING(), transitions={'Success':'FinishTask', 'Failure':'ErrorHandler', 'Fatal':'SafeMode'}, remapping={'input':'sm_data', 'output':'sm_data'})
# Add finishing step
smach.StateMachine.add('FinishTask', FinishTask.FINISHTASK(), transitions={'Success':'DONE', 'Failure':'ErrorHandler', 'Fatal':'SafeMode'}, remapping={'input':'sm_data', 'output':'sm_output'})
# Add manual mode
smach.StateMachine.add('ManualMode', ManualMode.MANUALMODE(), transitions={'Success':'ErrorHandler', 'Failure':'ErrorHandler', 'Fatal':'SafeMode'}, remapping={'input':'sm_data', 'output':'sm_data'})
# Add safe mode
smach.StateMachine.add('SafeMode', self.safemode, transitions={'Safed':'SAFE'}, remapping={'input':'sm_data', 'output':'sm_output'})
# Add Error handler
smach.StateMachine.add('ErrorHandler', ErrorHandler.ERRORHANDLER(), transitions={'ReFind':'FindValve', 'RePosition':'PositionRobot', 'RePlan':'PlanTurning', 'ReExecute':'ExecuteTurning', 'ReFinish':'FinishTask', 'GoManual':'ManualMode', 'Failed':'FAILED', 'Fatal':'SafeMode'}, remapping={'input':'sm_data', 'output':'sm_output'})
# Set up the introspection server
self.sis = smach_ros.IntrospectionServer('valve_task_smach_server', self.sm, '/SM_ROOT')
self.sis.start()
def init_ros():
''' ROS node initialization.
Initialize the node, advertise topics, and any other ROS housekeeping.
'''
global topics, param_handler
rospy.init_node('sickldmrs', log_level=rospy.DEBUG)
node_name = rospy.get_name().split('/')[-1]
rospy.loginfo('node %s starting up.' % node_name)
topics['cloud'] = rospy.Publisher('/%s/cloud' % node_name,
numpy_msg(PointCloud2))
topics['scan0'] = rospy.Publisher('/%s/scan0' % node_name,
numpy_msg(LaserScan))
topics['scan1'] = rospy.Publisher('/%s/scan1' % node_name,
numpy_msg(LaserScan))
topics['scan2'] = rospy.Publisher('/%s/scan2' % node_name,
numpy_msg(LaserScan))
topics['scan3'] = rospy.Publisher('/%s/scan3' % node_name,
numpy_msg(LaserScan))
rospy.on_shutdown(node_shutdown_hook)
def __init__(self):
rospy.on_shutdown(self.cleanup)
self.speed = 0.1
self.buildmap = False
self.follower = False
self.navigation = False
self.msg = Twist()
# Create the sound client object
self.soundhandle = SoundClient()
rospy.sleep(1)
self.soundhandle.stopAll()
# Announce that we are ready for input
rospy.sleep(1)
self.soundhandle.say('Hi, my name is Petrois')
rospy.sleep(3)
self.soundhandle.say("Say one of the navigation commands")
# publish to cmd_vel, subscribe to speech output
self.pub_ = rospy.Publisher("cmd_vel", Twist, queue_size=2)
rospy.Subscriber('recognizer/output', String, self.speechCb)
r = rospy.Rate(10.0)
while not rospy.is_shutdown():
self.pub_.publish(self.msg)
r.sleep()
def main():
"""Experiment module"""
box_fit = BoxFit()
camera = Webcam(box_fit.img_dir)
rospy.on_shutdown(box_fit.clean_shutdown)
box_fit.set_neutral()
box_fit.take_reference_images(camera)
box_fit.compress_object()
box_fit.process_images()
plotting = Plotting(box_fit.img_dir)
rostopic_filename = plotting.directory + 'rostopic_data.txt'
csv_filename = plotting.directory + 'sizes.csv'
webcam_data_filename = plotting.directory + 'webcam_data.txt'
timing_filename = plotting.directory + 'timestamps.txt'
plotting.parseTimingFile(timing_filename)
csvdict = plotting.parseCSV(csv_filename, webcam_data_filename)
#print csvdict
rostopicdict = plotting.parseRostopic(rostopic_filename)
#print rostopicdict
mergedict = plotting.mergeTiming(rostopicdict, csvdict)
savefile = plotting.directory + 'merge.csv'
plotting.saveAsCSV(savefile, mergedict)
def __init__(self):
rospy.init_node("position_test")
rospy.on_shutdown(self.shutdown)
rate = rospy.get_param("~rate", 100)
r = rospy.Rate(rate)
self.head_pan_joint = rospy.get_param("~head_pan_joint", "head_pan_joint")
self.joints = [self.head_pan_joint]
self.default_joint_speed = rospy.get_param("~default_joint_speed", 0.2)
self.joint_state = JointState()
rospy.Subscriber("joint_states", JointState, self.update_joint_state)
while not rospy.is_shutdown():
rospy.sleep(1)
rospy.loginfo("waiting for joint_states")
rospy.wait_for_message("joint_states", JointState)
current_pan = self.joint_state.position[self.joint_state.name.index(self.head_pan_joint)]
print "A"
print current_pan
print "B"
print current_pan * 180 / math.pi
def Serve(self,scan_msg, pose_msg):
print scan_msg.ranges
# time.sleep(0.1)
command2 = Twist()
if self.succeeded:
self.generateGoal(scan_msg, pose_msg)
rospy.on_shutdown(self.shutdown)
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("wait for the action server to come up: ")
self.move_base.wait_for_server(rospy.Duration(5))
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'odom'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose = Pose(Point(self.goal_x, self.goal_y, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
self.move_base.send_goal(goal)
success = self.move_base.wait_for_result(rospy.Duration(20))
if not success:
self.move_base.cancel_goal()
rospy.loginfo("The base failed to reach the desired pose")
self.succeeded =False
else:
# We made it!
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Hooray, reached the desired pose")
self.succeeded =True
def __init__(self, script_path):
rospy.init_node('talkback')
rospy.on_shutdown(self.cleanup)
# Set the default TTS voice to use
self.voice = rospy.get_param("~voice", "voice_don_diphone")
# Set the wave file path if used
self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds")
# Create the sound client object
self.soundhandle = SoundClient()
# Wait a moment to let the client connect to the
# sound_play server
rospy.sleep(1)
# Make sure any lingering sound_play processes are stopped.
self.soundhandle.stopAll()
# Announce that we are ready for input
self.soundhandle.playWave(self.wavepath + "/R2D2a.wav")
rospy.sleep(1)
self.soundhandle.say("Ready", self.voice)
rospy.loginfo("Say one of the navigation commands...")
# Subscribe to the recognizer output and set the callback function
rospy.Subscriber('/recognizer/output', String, self.talkback)
def __init__(self):
rospy.init_node('pilot', anonymous=True)
self.pub_takeoff = rospy.Publisher(
'/ardrone/takeoff', Empty, queue_size=1)
self.pub_land = rospy.Publisher('/ardrone/land', Empty, queue_size=1)
self.pub_reset = rospy.Publisher('/ardrone/reset', Empty, queue_size=1)
# Gazebo Reset Simulator
self.reset_world = rospy.ServiceProxy('/gazebo/reset_world', srv_Empty)
rospy.Subscriber("/gazebo/model_states", ModelStates,
self.update_navdata)
# self.pub_cmd = rospy.Publisher('/cmd_vel',Twist, 1)
self.pub_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.pos_x = 0
self.pos_y = 0
self.roll = 0
self.pitch = 0
self.yaw = 0
self.target_yaw = 270
self.command = Twist()
self.VelCommandTimer = rospy.Timer(rospy.Duration(
COMMAND_PERIOD / 1000.0), self.send_cmd)
rospy.on_shutdown(self.land)
def main():
"""RSDK End Effector Position Example: Keyboard Control
Use your dev machine's keyboard to control end effector positions.
"""
epilog = """
See help inside the example with the '?' key for key bindings.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__,
epilog=epilog)
parser.parse_args(rospy.myargv()[1:])
print("Initializing node... ")
rospy.init_node("rsdk_joint_position_keyboard")
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
def clean_shutdown():
print("\nExiting example...")
if not init_state:
print("Disabling robot...")
rs.disable()
rospy.on_shutdown(clean_shutdown)
print("Enabling robot... ")
rs.enable()
map_keyboard()
print("Done.")
def __init__(self):
self.x = 0
self.y = 0
self.z = 0
rospy.init_node("cmps09")
# Opens up serial port
self.port = rospy.get_param("~port", "")
self.hz = rospy.get_param("~hz", 20)
self.timeout = rospy.get_param("~timeout", 0.2)
if self.port != "":
rospy.loginfo("CMPS09 using port %s.", self.port)
else:
rospy.logerr("No port specified for CMPS09!")
exit(1)
try:
self.transport = serial.Serial(port=self.port, baudrate=9600, timeout=self.timeout)
self.transport.open()
except serial.serialutil.SerialException as e:
rospy.logerr("CMPS09: %s" % e)
exit(1)
# Registers shutdown handler to close the serial port we just opened.
rospy.on_shutdown(self.shutdown_handler)
# Registers as publisher
self.pub = rospy.Publisher("raw_mag", Point)
# rospy.Rate(10)
def myhook():
msg = drive_param()
angle = 0.
speed = -1.0
msg.angle = angle
msg.velocity = speed
pub.publish(msg)
msg = drive_param()
angle = 0.
speed = 0.
msg.angle = angle
msg.velocity = speed
pub.publish(msg)
msg = drive_param()
angle = 0.
speed = 0.
msg.angle = angle
msg.velocity = speed
pub.publish(msg)
if __name__ == "__main__":
rospy.init_node("pid_controller", anonymous=True)
rospy.Subscriber("path_planner", Float64MultiArray, desired_track)
rospy.Subscriber("slam_out_pose", PoseStamped, control)
rospy.on_shutdown(myhook)
rospy.spin()
derivative = erroradj - preverror
if erroradj > 0:
rot_speed = ((kp * erroradj) + (kd * derivative) + min_speed
) # turning right is positive value
else:
rot_speed = ((kp * erroradj) + (kd * derivative) - min_speed)
preverror = erroradj #cual es tu mejor choice para definir error?(afect derivative) el erroradj or el true error
#--------CONTROLLER ENDS HERE-----------------------------------------
# BUILD ARRAY message
speedmsg = Float32MultiArray()
speedmsg.data = [lin_speed, rot_speed]
pub_speeds.publish(speedmsg)
rate.sleep()
# INITIALIZE PUBLISHERS & SUBSCRIBERS
sub_theta = rospy.Subscriber('theta_gyro', Float32MultiArray,
Get_IMU_Data) # from IMU
sub_theta_ref = rospy.Subscriber('ref_theta', Float64,
Get_Reference) # from kbd or Xbox controller
pub_speeds = rospy.Publisher('speed_commands', Float32MultiArray, queue_size=1)
rospy.init_node('Controller_heading', anonymous=False)
rate = rospy.Rate(100) # 100 Hz sampling/publishing frequency
if __name__ == '__main__':
rospy.on_shutdown(cleanupOnExit)
control()
rospy.spin()
def __init__(self):
# Initialize
rospy.init_node('MakeSquare', anonymous=False)
# What to do you ctrl + c (call shutdown function written below)
rospy.on_shutdown(self.shutdown)
def __init__(self):
# Give the node a name
rospy.init_node('out_and_back', anonymous=False)
# 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_mux/input/navi', Twist, queue_size=5)
# How fast will we update the robot's movement?
rate = 20
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the forward linear speed to 0.15 meters per second
linear_speed = 0.15
# Set the travel distance in meters
goal_distance = 1.0
# Set the rotation speed in radians per second
angular_speed = 0.5
# Set the angular tolerance in degrees converted to radians
angular_tolerance = radians(1.0)
# Set the rotation angle to Pi radians (180 degrees)
goal_angle = pi
# 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()
# Loop once for each leg of the trip
for i in range(2):
# 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 the rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# 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
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 loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
# Stop the robot before the next leg
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Stop the robot for good
self.cmd_vel.publish(Twist())
def Mission_strategy():
global action, count_stop, Arm_state_flag, Sent_data_flag, image_point, cntr_point, item_name, get_image_flag, suck_point, change_position
global final_x, final_y, center_x, center_y, yolo_z, down_stop_flag, diff_item, picture_count, pressure_info, switch_flag
#print('in top of mission updown ', [Arm_state_flag, Sent_data_flag,Arm_state_flag == Arm_status.Idle and Sent_data_flag == 1])
# print([Arm_state_flag, Sent_data_flag])
if Arm_state_flag == Arm_status.Idle and Sent_data_flag == 1:
Sent_data_flag = 0
Arm_state_flag = Arm_status.Isbusy
for case in switch(action): #傳送指令給socket選擇手臂動作
if case(0): #Arm to BOX above
global is_metal, is_bottle, is_paper
print("---Arm to BOX above---")
is_metal = 0
is_bottle = 0
is_paper = 0
pos.x = 1.5
pos.y = 45.5
pos.z = star_config.yolo_z
pos.pitch = -90
pos.roll = 0
pos.yaw = 0
action = 1
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
ArmTask.Speed_Mode(1)
ArmTask.Arm_Mode(4, 1, 0, 30, 2) #speed up
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 100, 2) #action,ra,grip,vel,both
break
if case(1): #get_obj_coordinate & in postition
print("---get item information & ready to catch---")
time.sleep(0.75)
switch_flag = 0
get_image_flag = 1 #let yolo identify item
time.sleep(0.5)
get_image_flag = 0
realsense_look()
# show_orientation()
pos.x = pos.x
pos.y = pos.y
pos.z = -2.35
pos.pitch = -90
pos.roll = 0
pos.yaw = 0
sent_sucker_signal("on")
action = 2
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 100, 2) #action,ra,grip,vel,both
break
if case(2): #catch items in box
print("---catch item stage 1---")
sent_sucker_signal("on")
pos.x = cntr_point.center_point_x
pos.y = cntr_point.center_point_y
pos.z = -2.65
pos.pitch = -90
pos.roll = 0
pos.yaw = 0
action = 12
if cntr_point.center_point_y < 51:
if cntr_point.center_point_x > 1.5:
pos.yaw = -90
elif cntr_point.center_point_x < 1.5:
pos.yaw = 90
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 100, 2) #action,ra,grip,vel,both
break
if case(3): #down & suck the item
pos.x = pos.x
pos.y = pos.y
pos.z = pos.z - 0.4
pos.pitch = -90
pos.roll = 0
pos.yaw = pos.yaw
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 100, 2) #action,ra,grip,vel,both
# arduino_sucker_info()
if pos.z <= -26.35 or down_stop_flag == 0: # -13.45
print("---catch item stage 3---")
action = 4
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z,
' pitch: ', pos.pitch, ' roll: ', pos.roll, ' yaw: ',
pos.yaw)
break
if case(4): #arm up
#time.sleep(0.5)
print("---Arm up---")
pos.x = pos.x
pos.y = pos.y
pos.z = 0
pos.pitch = -90
pos.roll = 0
pos.yaw = pos.yaw
action = 5
if down_stop_flag == 1:
action = 0
# if suck_point < 4:
# suck_point = suck_point +1
# else:
# suck_point = 0
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
ArmTask.Speed_Mode(0)
ArmTask.Arm_Mode(4, 1, 0, 30, 2) #speed down
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 100, 2) #action,ra,grip,vel,both
#========================new function======================================#
if image_point.label == "bottle":
diff_item = 0
action = 7
elif image_point.label == "paper":
diff_item = 1
action = 7
elif image_point.label == "metal":
diff_item = 2
action = 7
else:
diff_item = 0
action = 7
print("cant see anything")
break
if case(5): #move to camera sight
print("---move to camera sight---")
pos.x = -13
pos.y = 55
pos.z = 1.35
pos.pitch = -90
pos.roll = 0
pos.yaw = 0
action = 6
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
ArmTask.Speed_Mode(1)
ArmTask.Arm_Mode(4, 1, 0, 10, 2) #speed up
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 50, 2) #action,ra,grip,vel,both
time.sleep(1) #please delete
break
if case(6): #analyst item & pos
global is_metal, is_bottle, is_paper
print("---analyst item---")
time.sleep(1)
action = 10
yolo_receive()
if image_point.label == "metal":
is_metal = is_metal + 1
elif image_point.label == "bottle":
is_bottle = is_bottle + 1
elif image_point.label == "paper":
is_paper = is_paper + 1
if is_metal == 2 or is_bottle == 2 or is_metal == 2: #if first two times is the same then direct identify item
identify_item()
action = 7
pos.x = -13
pos.y = 55
pos.z = 1.35
pos.pitch = -90
pos.roll = 0
pos.yaw = 0
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch,
pos.roll, pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 60, 2) #action,ra,grip,vel,both
elif picture_count >= 2: #maybe can delete?
identify_item()
action = 7
pos.x = -13
pos.y = 55
pos.z = 1.35
pos.pitch = -90
pos.roll = 0
pos.yaw = 0
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch,
pos.roll, pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 60, 2) #action,ra,grip,vel,both
picture_count = picture_count + 1
break
if case(7): #select final point
print("---select final point---")
picture_count = 0
if diff_item == 0: #bottle
final_x = 1.5
final_y = 35.5
print("Its bottle!")
action = 8
elif diff_item == 1: #paper
final_x = 33
final_y = 43.5
print("Its paper!")
action = 8
elif diff_item == 2: #metal
final_x = -24
final_y = 43.5
print("Its metal!")
action = 8
elif diff_item == 3:
action = 10
print("cant identify item!")
else:
print("unkown item")
break
if case(8): #move to final point
print("---move to final point---")
pos.x = final_x
pos.y = final_y
pos.z = star_config.yolo_z
pos.pitch = -90
pos.roll = 0
pos.yaw = pos.yaw
action = 9
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
ArmTask.Speed_Mode(1)
ArmTask.Arm_Mode(4, 1, 0, 30, 2) #speed up
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 60, 2) #action,ra,grip,vel,both
break
if case(9): #drop item to container & recycle
print("---drop item to container---")
if down_stop_flag == 0:
count_stop = count_stop + 1
time.sleep(0.75)
action = 0
sent_sucker_signal("off")
suck_point = 0
change_position = 0
print("round: ", count_stop)
break
#===============================================================================#
if case(10): #use when second camera get
print("---spin the item---")
pos.x = -13
pos.y = 55
pos.z = 1.35
pos.pitch = -90
pos.roll = 0
pos.yaw = pos.yaw + 30
action = 6
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 100, 2) #action,ra,grip,vel,both
break
if case(11): #ensure realsense not to look for same point
print("---change realsense position---")
if suck_point == 1: #realsense to front
pos.x = 1.5
pos.y = 48.5
pos.z = star_config.yolo_z
elif suck_point == 2: #realsense to rear
pos.x = 1.5
pos.y = 42.5
pos.z = star_config.yolo_z
elif suck_point == 3: #realsense to front(down)
pos.x = 1.5
pos.y = 45.5
pos.z = -4.65
elif suck_point == 4: #realsense to rear(down)
pos.x = 1.5
pos.y = 42.5
pos.z = -4.65
else: #realsense to original position
pos.x = 1.5
pos.y = 45.5
pos.z = star_config.yolo_z
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 60, 2) #action,ra,grip,vel,both
action = 1
if case(12): #catch items in box(down)
print("---catch item stage 2---")
pos.x = cntr_point.center_point_x
pos.y = cntr_point.center_point_y
pos.z = cntr_point.z_depth
pos.pitch = -90
pos.roll = 0
pos.yaw = pos.yaw
action = 3
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 80, 2) #action,ra,grip,vel,both
break
if case(): # default, could also just omit condition or 'if True'
rospy.on_shutdown(myhook)
ArmTask.rospy.on_shutdown(myhook)
def __init__(self):
# Initializing publisher with buffer size of 10 messages
self.pub_ = rospy.Publisher("recognized", String, queue_size=10)
# initialize node
rospy.init_node("asr_control")
# Call custom function on node shutdown
rospy.on_shutdown(self.shutdown)
# Params
# File containing language model
_lm_param = "~lm"
# Dictionary
_dict_param = "~dict"
# Hidden markov model. Default has been provided below
_hmm_param = "~hmm"
# Gram file contains the rules and grammar
_gram = "~gram"
# Name of rule within the grammar
_rule = "~rule"
_grammar = "~grammar"
# check if lm or grammar mode. Default = grammar
self._use_lm = 0
self.in_speech_bf = False
# Setting param values
if rospy.has_param(_hmm_param):
self.hmm = rospy.get_param(_hmm_param)
if rospy.get_param(_hmm_param) == ":default":
if os.path.isdir(
"/usr/local/lib/python2.7/dist-packages/pocketsphinx/model/"
):
rospy.loginfo("Loading the default acoustic model")
self.hmm = "/usr/local/lib/python2.7/dist-packages/pocketsphinx/model/en-us/"
rospy.loginfo("Done loading the default acoustic model")
else:
rospy.logerr(
"No language model specified. Couldn't find default model."
)
return
else:
rospy.logerr(
"No language model specified. Couldn't find default model.")
return
if rospy.has_param(
_dict_param) and rospy.get_param(_dict_param) != ":default":
self.dict = rospy.get_param(_dict_param)
else:
rospy.logerr(
"No dictionary found. Please add an appropriate dictionary argument."
)
return
if rospy.has_param(
_grammar) and rospy.get_param(_grammar) != ":default":
pass
else:
rospy.logerr(
"No grammar found. Please add an appropriate grammar along with gram file."
)
return
if rospy.has_param(
_lm_param) and rospy.get_param(_lm_param) != ':default':
self._use_lm = 1
self.class_lm = rospy.get_param(_lm_param)
elif rospy.has_param(_gram) and rospy.has_param(_rule):
self._use_lm = 0
self.gram = rospy.get_param(_gram)
self.rule = rospy.get_param(_rule)
else:
rospy.logerr(
"Couldn't find suitable parameters. Please take a look at the documentation"
)
return
# All params satisfied. Starting recognizer
self.start_recognizer()
def __init__(self):
## Init node
rospy.init_node("uisee_driver")
# Shutdown function
rospy.on_shutdown(self.shutdown)
## Get Parameters
# joy axes and buttons index
self.left_joy_lr = rospy.get_param("~left_joy_lr", 0)
self.Y = rospy.get_param("Y", 0)
self.B = rospy.get_param("B", 1)
self.A = rospy.get_param("A", 2)
self.X = rospy.get_param("X", 3)
#self.LB = rospy.get_param("LB", 4)
self.RB = rospy.get_param("RB", 5)
self.LT = rospy.get_param("LT", 6)
self.RT = rospy.get_param("RT", 7)
# the amount of change of velocity
self.v_delta = rospy.get_param("~v_delta", 10.0)
self.v_c = rospy.get_param("~v_c", 30.0)
self.s_turn = rospy.get_param("~s_turn", 2.0)
# gear interval
self.v_min = rospy.get_param("~v_min", 15.0)
self.v_max = rospy.get_param("~v_max", 45.0)
self.w1_scale = rospy.get_param("~gear_w1", 3.0)
self.w2_scale = rospy.get_param("~gear_w2", 5.0)
self.w3_scale = rospy.get_param("~gear_w3", 10.0)
# for local computer, use 127.0.0.1
target_ip = rospy.get_param("~target_ip", '192.168.1.108')
# img
self.data_path = rospy.get_param("~data_path", './dataset/img/')
self.count = rospy.get_param("~img_count", 0)
self.num_frames = rospy.get_param("~num_frames", 10)
## Init Variables
self.steer = 0.0
#self.last_speed = 0.0
self.gear_w1_pressed = False
self.gear_w2_pressed = False
self.gear_w3_pressed = False
self.data_flag = False
## Uisee Simulator
#connection
self.id = usimpy.UsimCreateApiConnection(target_ip, 17771, 5000, 10000)
ret = usimpy.UsimStartSim(self.id, 10000)
print(ret)
#control
self.control = usimpy.UsimSpeedAdaptMode()
#states
self.states = usimpy.UsimVehicleState()
#collision
self.collision = usimpy.UsimCollision()
#image
self.image = usimpy.UsimCameraResponse()
# Joy subscriber (global topic. Make sure the joy node has been run)
self.joy_sub = rospy.Subscriber("joy",
Joy,
self.joy_callback,
queue_size=10)
# start a timer to publish control msg in 10hz
self.timer_pub = rospy.Timer(rospy.Duration(0.1), self.cmd_publish)
def __init__(self):
rospy.init_node('GpsrSmach')
rospy.on_shutdown(self.shutdown)
rospy.logerr('you are all sillyb')
self.smach_bool = False
# xm_arm_moveit_name('nav_pose')#最开始时，把机械臂降下来
self.sm_EnterRoom = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_EnterRoom:
# if use arm, use this state
# self.sm_EnterRoom.userdata.arm_mode_1 =0
# self.sm_EnterRoom.userdata.arm_ps = PointStamped()
# StateMachine.add('NAV_POSE',
# ArmCmd(),
# transitions={'succeeded':'DOOR_DETECT','aborted':'NAV_POSE','error':'error'},
# remapping ={'arm_ps':'arm_ps','mode':'arm_mode_1'})
#first detect the door is open or not
StateMachine.add('DOOR_DETECT',
DoorDetect().door_detect_,
transitions={
'invalid': 'WAIT',
'valid': 'DOOR_DETECT',
'preempted': 'aborted'
})
# after clear the costmap, wait for some seconds
self.sm_EnterRoom.userdata.rec = 3.0
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'SIMPLE_MOVE',
'error': 'error'
},
remapping={'rec': 'rec'})
# before executing the nav-stack, move directly through the door may make the nav-stack easier
self.sm_EnterRoom.userdata.go_point = Point(0.1, 0.0, 0.0)
StateMachine.add('SIMPLE_MOVE',
SimpleMove_move(),
remapping={'point': 'go_point'},
transitions={
'succeeded': 'NAV_1',
'aborted': 'NAV_1',
'error': 'error'
})
# go to the known Pose in the map to get the gpsr-command
self.sm_EnterRoom.userdata.start_waypoint = gpsr_target['speaker'][
'pos']
StateMachine.add('NAV_1',
NavStack(),
transitions={
'succeeded': 'succeeded',
'aborted': 'NAV_1',
'error': 'error'
},
remapping={'pos_xm': 'start_waypoint'})
self.sm_Find = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
# 寻找人
with self.sm_Find:
# for the people-finding task, pay attention that at the end of the task,
# run a simple executable file to make the cv-node release the KinectV2
# otherwise, the find-object task may throw error and all the cv-node may
# break down
self.sm_Person = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_Person:
self.sm_Person.userdata.rec = 2.0
# run the people_tracking node
StateMachine.add('RUNNODE',
RunNode(),
transitions={
'succeeded': 'WAIT',
'aborted': 'succeeded'
})
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'GET_PEOPLE_POS',
'error': 'error'
},
remapping={'rec': 'rec'})
#the data should be PointStamped()
# 这里必须先运行节点，也就是用RunNode()状态
self.sm_Person.userdata.pos_xm = Pose()
StateMachine.add('GET_PEOPLE_POS',
FindPeople().find_people_,
transitions={
'invalid': 'NAV_PEOPLE',
'valid': 'SPEAK',
'preempted': 'aborted'
},
remapping={'pos_xm': 'pos_xm'})
# this state will use the userdata remapping in the last state
StateMachine.add('NAV_PEOPLE',
NavStack(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'NAV_PEOPLE',
'error': 'error'
},
remapping={'pos_xm': 'pos_xm'})
self.sm_Person.userdata.sentences = 'I find you'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'CLOSEKINECT',
'error': 'error'
},
remapping={'sentences': 'sentences'})
# close the KinectV2
StateMachine.add('CLOSEKINECT',
CloseKinect(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
self.sm_Pos = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
with self.sm_Pos:
# this command may be foolish, but it may be used in the next competition
# get the name of the target
# in the target_gpsr.py we define some Pose() , they are all named as ${room_name}+'_table_1(2)'
# because each room we have some best detecting positions, we should make xm go to these places
# for object-find tasks
self.sm_Pos.userdata.pose = Pose()
self.sm_Pos.userdata.mode_1 = 1
StateMachine.add('GET_POS',
GetPos(),
remapping={
'target': 'target',
'current_task': 'current_task',
'pose': 'pose',
'mode': 'mode_1'
},
transitions={
'succeeded': 'NAV_HEHE',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('NAV_HEHE',
NavStack(),
remapping={'pos_xm': 'pose'},
transitions={
'succeeded': 'GET_TARGET',
'aborted': 'NAV_HEHE',
'error': 'error'
})
self.sm_Pos.userdata.target_mode = 0
self.sm_Pos.userdata.name = ''
StateMachine.add('GET_TARGET',
GetTarget(),
remapping={
'target': 'target',
'current_task': 'current_task',
'mode': 'target_mode',
'current_target': 'name'
},
transitions={
'succeeded': 'FIND_OBJECT',
'aborted': 'aborted',
'error': 'error'
})
self.sm_Pos.userdata.object_pos = PointStamped()
StateMachine.add('FIND_OBJECT',
FindObject(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'GET_POS_1',
'error': 'error'
},
remapping={
'name': 'name',
'object_pos': 'object_pos'
})
# if xm not finds the object in the first detecting-place, she should go to the second
# specified place for detect-task, but as U can see in the target_gpsr.py, some room are
# only one best detecting-place
self.sm_Pos.userdata.pose_1 = Pose()
self.sm_Pos.userdata.mode_2 = 2
StateMachine.add('GET_POS_1',
GetPos(),
remapping={
'target': 'target',
'current_task': 'current_task',
'pose': 'pose_1',
'mode': 'mode_2'
},
transitions={
'succeeded': 'NAV_HAHA',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('NAV_HAHA',
NavStack(),
remapping={'pos_xm': 'pose_1'},
transitions={
'succeeded': 'FIND_OBJECT_1',
'aborted': 'NAV_HAHA',
'error': 'error'
})
self.sm_Pos.userdata.object_pos = PointStamped()
StateMachine.add('FIND_OBJECT_1',
FindObject(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'SPEAK',
'error': 'error'
},
remapping={
'name': 'name',
'object_pos': 'object_pos'
})
self.sm_Pos.userdata.sentences = 'I find it'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'succeeded',
'error': 'error'
},
remapping={'sentences': 'sentences'})
# this state is used for swithing the mode, finding people or finding object?
StateMachine.add('PERSON_OR_POS',
PersonOrPosition(),
transitions={
'person': 'PERSON',
'position': 'POS',
'error': 'error'
},
remapping={
'target': 'target',
'current_task': 'current_task'
})
StateMachine.add('PERSON',
self.sm_Person,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('POS',
self.sm_Pos,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'target': 'target',
'current_task': 'current_task'
})
#nav tasks
self.sm_Nav = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
with self.sm_Nav:
self.sm_Nav.userdata.pos_xm = Pose()
self.sm_Nav.userdata.target_mode = 1
StateMachine.add('GETTARGET',
GetTarget(),
transitions={
'succeeded': "NAV_GO",
'aborted': 'aborted',
'error': 'error'
},
remapping={
'target': 'target',
'current_task': "current_task",
'current_target': 'pos_xm',
'mode': 'target_mode'
})
# StateMachine.add('FINDWAY',
# FindWay(),
# transitions = {'succeeded':'NAV_PATH1','aborted':'NAV_GO','error':'NAV_GO'},
# remapping={'way_path1':'way_path1','way_path2':'way_path2'})
# StateMachine.add('NAV_PATH1',
# NavStack(),
# transitions={'succeeded':'NAV_PATH2','aborted':'NAV_PATH1','error':'error'},
# remapping={'pos_xm':'way_path1'})
# StateMachine.add('NAV_PATH2',
# NavStack(),
# transitions = {'succeeded':'NAV_GO','aborted':'NAV_PATH2','error':'error'},
# remapping={'pos_xm':'way_path2'})
StateMachine.add('NAV_GO',
NavStack(),
transitions={
'succeeded': 'SPEAK',
'aborted': 'NAV_GO',
'error': 'error'
},
remapping={'pos_xm': 'pos_xm'})
self.sm_Nav.userdata.sentences = 'I arrive here'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'succeeded',
'error': 'error'
},
remapping={'sentences': 'sentences'})
#follow task
# we will use the concurrence fun
# the tutorials in the wiki too easy, it is no help for you to understand the concurrence
# please see the test-snippet of the source of executive-smach
# https://github.com/ros/executive_smach/blob/indigo-devel/smach_ros/test/concurrence.py
self.sm_Follow = Concurrence(outcomes=['succeeded', 'aborted'],
default_outcome='succeeded',
outcome_map={
'succeeded': {
'STOP': 'stop'
},
'aborted': {
'FOLLOW': 'aborted'
}
},
child_termination_cb=self.child_cb)
with self.sm_Follow:
self.meta_follow = StateMachine(outcomes=['succeeded', 'aborted'])
with self.meta_follow:
StateMachine.add('FOLLOW',
SimpleFollow(),
transitions={
'succeeded': 'FOLLOW',
'aborted': 'aborted',
'preempt': 'succeeded'
})
Concurrence.add('FOLLOW', self.meta_follow)
Concurrence.add('STOP', CheckStop())
Concurrence.add('RUNNODE', RunNode())
# self.meta_Follow = StateMachine(outcomes =['succeeded','aborted','error'])
# with self.meta_Follow:
# StateMachine.add('RUNNODE',
# RunNode(),
# transitions={'succeeded':'FIND_PEOPLE','aborted':'succeeded'})
# self.meta_Follow.userdata.pos_xm = Pose()
# StateMachine.add('FIND_PEOPLE',
# FindPeople().find_people_,
# remapping ={'pos_xm':'pos_xm'},
# transitions ={'invalid':'MOVE','valid':'FIND_PEOPLE','preempted':'aborted'})
# StateMachine.add('MOVE',
# NavStack(),
# remapping ={'pos_xm':'pos_xm'},
# transitions={'succeeded':'FIND_PEOPLE','aborted':'MOVE','error':'error'})
# self.meta_Stop = StateMachine(outcomes =['succeeded','aborted'])
# with self.meta_Stop:
# StateMachine.add('STOP',
# StopFollow(),
# transitions ={'succeeded':'succeeded','aborted':'STOP'})
# Concurrence.add('FOLLOW',
# self.meta_Follow)
# Concurrence.add('STOP',
# self.meta_Stop)
# speak task
self.sm_Talk = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.sm_Talk:
self.sm_Talk.userdata.people_condition = list()
StateMachine.add('SPEAK',
Anwser(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
self.sm_Pick = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
with self.sm_Pick:
self.sm_Pick.userdata.name = ''
self.sm_Pick.userdata.target_mode = 0
self.sm_Pick.userdata.objmode = -1
StateMachine.add('RUNNODE_IMG',
RunNode_img(),
transitions={
'succeeded': 'GETNAME',
'aborted': 'aborted'
})
StateMachine.add('GETNAME',
GetTarget(),
remapping={
'target': 'target',
'current_task': 'current_task',
'mode': 'target_mode',
'current_target': 'name'
},
transitions={
'succeeded': 'FIND_OBJECT',
'aborted': 'aborted',
'error': 'error'
})
self.sm_Pick.userdata.object_pos = PointStamped()
StateMachine.add('FIND_OBJECT',
FindObject(),
transitions={
'succeeded': 'POS_JUSTFY',
'aborted': 'FIND_OBJECT',
'error': 'SPEAK'
},
remapping={
'name': 'name',
'object_pos': 'object_pos',
'objmode': 'objmode'
})
#making the xm foreward the object may make the grasping task easier
self.sm_Pick.userdata.pose = Pose()
StateMachine.add('POS_JUSTFY',
PosJustfy(),
remapping={
'object_pos': 'object_pos',
'pose': 'pose'
},
transitions={
'succeeded': 'NAV_TO',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('NAV_TO',
NavStack(),
transitions={
'succeeded': 'RUNNODE_IMG2',
'aborted': 'NAV_TO',
'error': 'error'
},
remapping={"pos_xm": 'pose'})
StateMachine.add('RUNNODE_IMG2',
RunNode_img(),
transitions={
'succeeded': 'FIND_AGAIN',
'aborted': 'aborted'
})
StateMachine.add('FIND_AGAIN',
FindObject(),
transitions={
'succeeded': 'PICK',
'aborted': 'FIND_AGAIN',
'error': 'SPEAK'
},
remapping={
'name': 'name',
'object_pos': 'object_pos',
'objmode': 'objmode'
})
self.sm_Pick.userdata.arm_mode_1 = 1
StateMachine.add('PICK',
ArmCmd(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'arm_ps': 'object_pos',
'mode': 'arm_mode_1'
})
self.sm_Pick.userdata.sentences = 'xiao meng can not find things'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
})
self.sm_Place = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_Place:
# place_ps please specified due to the scene
self.sm_Place.userdata.place_ps = PointStamped()
self.sm_Place.userdata.place_ps.header.frame_id = 'base_link'
self.sm_Place.userdata.place_ps.point.x = 0.8
self.sm_Place.userdata.place_ps.point.y = 0.0
self.sm_Place.userdata.place_ps.point.z = 0.6
self.sm_Place.userdata.objmode = 2
# without moveit, if is just place it in a open space
self.sm_Place.userdata.arm_mode_1 = 2
StateMachine.add('PLACE',
ArmCmd(),
transitions={
'succeeded': 'succeeded',
'aborted': 'PLACE',
'error': 'error'
},
remapping={
'arm_ps': 'place_ps',
'mode': 'arm_mode_1'
})
self.sm_GPSR = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.sm_GPSR:
self.sm_GPSR.userdata.target = list()
self.sm_GPSR.userdata.action = list()
self.sm_GPSR.userdata.task_num = 0
self.sm_GPSR.userdata.current_task = -1
self.sm_GPSR.userdata.current_turn = 1
self.sm_GPSR.userdata.turn = 3
self.sm_GPSR.userdata.pos_xm_door = gpsr_target['speaker']['pos']
self.sm_GPSR.userdata.pos_xm_exit = gpsr_target['exit']['pos']
self.sm_GPSR.userdata.sentences = 'please command me'
StateMachine.add('SPEAK_RESTART',
Speak(),
transitions={
'succeeded': 'RECEIVE_TASKS',
'aborted': 'RECEIVE_TASKS',
'error': 'RECEIVE_TASKS'
})
# get the task due to the speech-node
StateMachine.add('RECEIVE_TASKS',
GetTask(),
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'RECEIVE_TASKS',
'error': 'error'
},
remapping={
'target': 'target',
'action': 'action',
'task_num': 'task_num'
})
#get the task in order and execute the task
# if want to go to the door at the end of the task, please modify:
# succeeded->succeeded --> succeeded->BYEBYE
StateMachine.add('GET_NEXT_TASK',
NextDo(),
transitions={
'succeeded': 'CHECK_TURN',
'aborted': 'aborted',
'error': 'error',
'find': 'FIND',
'go': 'GO',
'follow': 'FOLLOW',
'pick': 'GET_NEXT_TASK',
'place': 'GET_NEXT_TASK',
'talk': 'TALK',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'action': 'action',
'current_task': 'current_task',
'task_num': 'task_num'
})
StateMachine.add('CHECK_TURN',
CheckTurn(),
transitions={
'succeeded': 'BACK_EXIT',
'continue': 'BACK_DOOR',
'error': 'error'
})
StateMachine.add('BACK_DOOR',
NavStack(),
transitions={
'succeeded': 'CHECK_TURN',
'aborted': 'BACK_DOOR',
'error': 'error'
},
remapping={'pos_xm': 'pos_xm_door'})
StateMachine.add('BACK_EXIT',
NavStack(),
transitions={
'succeeded': 'succeeded',
'aborted': 'BACK_EXIT',
'error': 'error'
},
remapping={'pos_xm': 'pos_xm_exit'})
# all the task-group
StateMachine.add('GO',
self.sm_Nav,
remapping={
'target': 'target',
'current_task': 'current_task'
},
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'GO'
})
StateMachine.add('FIND',
self.sm_Find,
remapping={
'target': 'target',
'current_task': 'current_task'
},
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'FIND'
})
StateMachine.add('FOLLOW',
self.sm_Follow,
transitions={
'succeeded': 'CLOSE',
'aborted': 'FOLLOW'
})
StateMachine.add('CLOSE',
CloseKinect(),
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'GET_NEXT_TASK'
})
StateMachine.add('PICK',
self.sm_Pick,
remapping={
'target': 'target',
'current_task': 'current_task'
},
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'PICK'
})
StateMachine.add('TALK',
self.sm_Talk,
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'TALK'
})
StateMachine.add('PLACE',
self.sm_Place,
transitions={
'succeeded': 'GET_NEXT_TASK',
'aborted': 'PLACE'
})
self.sm_GPSR.userdata.string = 'I finish all the three tasks, I will go out the stage'
StateMachine.add('BYEBYE',
Speak(),
transitions={
'succeeded': 'GOOUT',
'error': 'error'
},
remapping={'sentences': 'string'})
#no need
self.sm_GPSR.userdata.waypoint = gpsr_target['end']['pos']
StateMachine.add('GOOUT',
NavStack(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={"pos_xm": 'waypoint'})
intro_server = IntrospectionServer('enter_room', self.sm_EnterRoom,
'/SM_ROOT')
intro_server.start()
out_1 = self.sm_EnterRoom.execute()
intro_server.stop()
#只运行一次
intro_server = IntrospectionServer('sm_gpsr', self.sm_GPSR, '/SM_ROOT')
intro_server.start()
out_2 = self.sm_GPSR.execute()
intro_server.stop()
self.smach_bool = True
def __init__(self):
#give the node a name
rospy.init_node('calibrate_linear', 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?
self.rate = 10
r = rospy.Rate(self.rate)
#set the distance to travel
self.test_distance = 1.5
self.speed = 0.2
self.tolerance = 0.01
self.odom_linear_scale_correction = 1.0
self.start_test = True
#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 robot
self.base_frame = rospy.get_param('~base_frame', '/base_footprint')
#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)
#make sure we see the odom and base frames
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame,
rospy.Time(), rospy.Duration(60.0))
self.position = Point()
#get the starting position from the tf transform between the odom and base frames
self.position = self.get_position()
x_start = self.position.x
y_start = self.position.y
move_cmd = Twist()
while not rospy.is_shutdown():
#Stop the robot by default
move_cmd = Twist()
if self.start_test:
#get the current position from the tf transform between the odom and base frames
self.position = self.get_position()
#compute the euclidean distance from the target point
distance = sqrt(
pow((self.position.x - x_start), 2) +
pow((self.position.y - y_start), 2))
#correct the estimate distance by the correction factor
distance *= self.odom_linear_scale_correction
#How close are we?
error = distance - self.test_distance
#are we close enough?
if not self.start_test or abs(error) < self.tolerance:
self.start_test = False
params = False
rospy.loginfo(params)
else:
#if not, move in the appropriate direction
move_cmd.linear.x = copysign(self.speed, -1 * error)
else:
self.position = self.get_position()
x_start = self.position.x
y_start = self.position.y
self.cmd_vel.publish(move_cmd)
r.sleep()
#stop the robot
self.cmd_vel.publish(Twist())
def __init__(self):
self.datainput = Datainput()
self.pub_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
rospy.on_shutdown(self.shutdown)
self.flag = 2
def exec_music(goal):
r = MusicResult()
fb = MusicFeedback()
for i, f in enumerate(goal.freqs):
fb.remaining_steps = len(goal.freqs) - i
music.publish_feedback(fb)
if music.is_preempt_requested():
write_freq(0)
r.finished = False
music.set_preempted(r)
return
write_freq(f)
rospy.sleep(1.0 if i >= len(goal.durations) else goal.durations[i])
r.finished = True
music.set_succeeded(r)
def recv_buzzer(data):
write_freq(data.data)
if __name__ == '__main__':
rospy.init_node('buzzer')
rospy.Subscriber("buzzer", UInt16, recv_buzzer)
music = actionlib.SimpleActionServer('music', MusicAction, exec_music, False) # 追加
music.start() # 追加
rospy.on_shutdown(write_freq) # 追加
rospy.spin()
def __init__(self):
# Give the node a name
rospy.init_node('calibrate_linear', 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?
self.rate = rospy.get_param('~rate', 20)
r = rospy.Rate(self.rate)
# Set the distance to travel
self.test_distance = rospy.get_param('~test_distance', 1.0) # meters
self.speed = rospy.get_param('~speed', 0.15) # meters per second
self.tolerance = rospy.get_param('~tolerance', 0.01) # meters
self.odom_linear_scale_correction = rospy.get_param(
'~odom_linear_scale_correction', 1.0)
self.start_test = rospy.get_param('~start_test', True)
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# Fire up the dynamic_reconfigure server
dyn_server = Server(CalibrateLinearConfig,
self.dynamic_reconfigure_callback)
# Connect to the dynamic_reconfigure server
dyn_client = dynamic_reconfigure.client.Client("calibrate_linear",
timeout=60)
# 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)
# Make sure we see the odom and base frames
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame,
rospy.Time(), rospy.Duration(60.0))
rospy.loginfo("Bring up rqt_reconfigure to control the test.")
self.position = Point()
# Get the starting position from the tf transform between the odom and base frames
self.position = self.get_position()
x_start = self.position.x
y_start = self.position.y
move_cmd = Twist()
while not rospy.is_shutdown():
# Stop the robot by default
move_cmd = Twist()
if self.start_test:
# Get the current position from the tf transform between the odom and base frames
self.position = self.get_position()
# Compute the Euclidean distance from the target point
distance = sqrt(
pow((self.position.x - x_start), 2) +
pow((self.position.y - y_start), 2))
# Correct the estimated distance by the correction factor
distance *= self.odom_linear_scale_correction
# How close are we?
error = distance - self.test_distance
# Are we close enough?
if not self.start_test or abs(error) < self.tolerance:
self.start_test = False
params = {'start_test': False}
rospy.loginfo(params)
dyn_client.update_configuration(params)
else:
# If not, move in the appropriate direction
move_cmd.linear.x = copysign(self.speed, -1 * error)
else:
self.position = self.get_position()
x_start = self.position.x
y_start = self.position.y
self.cmd_vel.publish(move_cmd)
r.sleep()
# Stop the robot
self.cmd_vel.publish(Twist())
# print pose_target
# self.group.set_goal_tolerance(0.1)
self.group.set_pose_target(pose_target)
# self.group.set_position_target((msg.data[0],msg.data[1],msg.data[2]))
self.plan = self.group.plan()
# print self.group.plan()
ok = self.group.execute(self.plan, wait=True)
print ok
return ok
# if ok:
# break
# # print self.plan
# p += 0.01
# print p
def onShutdown(self):
rospy.loginfo("[%s] Shutting down..." % self.node_name)
rospy.sleep(0.5) #To make sure that it gets published.
rospy.loginfo("[%s] Shutdown" % self.node_name)
if __name__ == '__main__':
rospy.init_node('block_pick_and_place', anonymous=False)
app = QApplication(sys.argv)
block_pick_and_place = block_pick_and_place()
block_pick_and_place.show()
app.exec_()
rospy.on_shutdown(block_pick_and_place.onShutdown)
rospy.spin()
# rospy.loginfo("[%s] node %s msg %s" %(self.node_name, node_name, msg))
# rospy.loginfo("[%s] Node %s set to %s." %(self.node_name, node_name, node_state))
self.active_nodes = copy.deepcopy(active_nodes)
def cbEvent(self,msg,event_name):
if (msg.data == self.event_trigger_dict[event_name]):
# Update timestamp
rospy.loginfo("[%s] Event: %s" %(self.node_name,event_name))
self.state_msg.header.stamp = msg.header.stamp
next_state = self._getNextState(self.state_msg.state,event_name)
if next_state is not None:
# Has a defined transition
self.state_msg.state = next_state
self.publish()
def on_shutdown(self):
rospy.loginfo("[%s] Shutting down." %(self.node_name))
if __name__ == '__main__':
# Initialize the node with rospy
rospy.init_node('fsm_node', anonymous=False)
# Create the NodeName object
node = FSM()
# Setup proper shutdown behavior
rospy.on_shutdown(node.on_shutdown)
# Keep it spinning to keep the node alive
rospy.spin()
def Mission_Trigger():
global action, Arm_state_flag, Sent_data_flag
if Arm_state_flag == Arm_status.Idle and Sent_data_flag == 1:
Sent_data_flag = 0
Arm_state_flag = Arm_status.Isbusy
for case in switch(action): #傳送指令給socket選擇手臂動作
if case(0):
pos.x = 10
pos.y = 36.8
pos.z = 11.35
pos.pitch = -90
pos.roll = 0
pos.yaw = 0
action = 1
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
#ArmTask.strategy_client_Arm_Mode(2,1,0,10,2)#action,ra,grip,vel,both
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 10, 2) #action,ra,grip,vel,both
break
if case(1):
pos.x = 10
pos.y = 42
pos.z = 11.35
pos.pitch = -90
pos.roll = 0
pos.yaw = 0
action = 2
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
#ArmTask.strategy_client_Arm_Mode(2,1,0,10,2)#action,ra,grip,vel,both
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 10, 2) #action,ra,grip,vel,both
break
if case(2):
pos.x = -10
pos.y = 42
pos.z = 11.35
pos.pitch = -90
pos.roll = 0
pos.yaw = 0
action = 3
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
#ArmTask.strategy_client_Arm_Mode(2,1,0,10,2)#action,ra,grip,vel,both
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 10, 2) #action,ra,grip,vel,both
break
if case(3):
pos.x = -10
pos.y = 36.8
pos.z = 11.35
pos.pitch = -90
pos.roll = 0
pos.yaw = 0
action = 4
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
#ArmTask.strategy_client_Arm_Mode(2,1,0,10,2)#action,ra,grip,vel,both
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 10, 2) #action,ra,grip,vel,both
break
if case(4):
pos.x = 0
pos.y = 36.8
pos.z = 11.35
pos.pitch = -90
pos.roll = 0
pos.yaw = 0
action = 0
print('x: ', pos.x, ' y: ', pos.y, ' z: ', pos.z, ' pitch: ',
pos.pitch, ' roll: ', pos.roll, ' yaw: ', pos.yaw)
#ArmTask.strategy_client_Arm_Mode(2,1,0,10,2)#action,ra,grip,vel,both
ArmTask.point_data(pos.x, pos.y, pos.z, pos.pitch, pos.roll,
pos.yaw)
ArmTask.Arm_Mode(2, 1, 0, 10, 2) #action,ra,grip,vel,both
break
if case(): # default, could also just omit condition or 'if True'
rospy.on_shutdown(myhook)
ArmTask.rospy.on_shutdown(myhook)
s = self.sensor_values
data.linear.x += accel
if self.sensor_values.sum_all >= 50:
data.linear.x = 0.0
elif data.linear.x <= 0.2:
data.linear.x = 0.2
elif data.linear.x >= 0.8:
data.linear.x = 0.8
if data.linear.x < 0.2:
data.angular.z = 0.0
elif s.left_side < 10:
data.angular.z = 0.0
else:
target = 50
error = (target - s.left_side) / 50.0
data.angular.z = error * 3 * math.pi / 180.0
self.cmd_vel.publish(data)
rate.sleep()
if __name__ == '__main__':
rospy.init_node('wall_Trace')
rospy.wait_for_service('/motor_on')
rospy.wait_for_service('/motor_off')
rospy.on_shutdown(rospy.ServiceProxy('/motor_off', Trigger).call)
rospy.ServiceProxy('/motor_on', Trigger).call()
WallTrace().run()
veh_name=self.veh_name)
self.pub_skeletons.publish(marker_array)
def callback_beziers(self, beziers_msg):
marker_array = beziers_to_marker_array(beziers_msg,
veh_name=self.veh_name)
self.pub_beziers.publish(marker_array)
def callback_waypoint(self, waypoint_msg):
point_msg = PointStamped()
point_msg.header.frame_id = self.veh_name
point_msg.point = waypoint_msg
self.pub_waypoint.publish(point_msg)
def loginfo(self, message):
rospy.loginfo('[{0}] {1}'.format(self.node_name, message))
def setup_parameter(self, name, default):
value = rospy.get_param(name, default)
return value
def on_shutdown(self):
self.loginfo('Shutting down')
if __name__ == '__main__':
rospy.init_node('merganser_visualization_node', anonymous=False)
visualization_node = VisualizationNode()
rospy.on_shutdown(visualization_node.on_shutdown)
rospy.spin()
print("Pick_home service already!!")
self.task2 = rospy.Service('task2', task2_srv, self.task2_ser)
rospy.wait_for_service('pose_estimation')
self.pose_service = rospy.ServiceProxy('/pose_estimation',
pose_estimation)
print("Pose_estimation service already!!")
rospy.wait_for_service('pick')
self.pick_service = rospy.ServiceProxy('/pick', pick_srv)
print("Picking service already!!")
def task2_ser(self, req):
pick_home_res = self.pick_service()
pose_msg = self.pose_service()
for i in range(len(pose_msg.obj_list)):
result = self.pick_service(pose_msg.pose[i])
if result.result:
return task2_srvResponse(pose_msg.tagID[i])
return task2_srvResponse("Task 2 is Fail")
def onShutdown(self):
rospy.loginfo("Shutdown.")
if __name__ == '__main__':
rospy.init_node('client_task2', anonymous=False)
rospy.sleep(2)
client_task2 = client_task2()
rospy.on_shutdown(client_task2.onShutdown)
rospy.spin()
def __init__(self):
''' Initiate self and subscribe to /r_values topic '''
# controller variables
# with open('variables.yaml') as f:
# data2 = yaml.load(f, Loader=yaml.FullLoader)
self.running = np.float32(1)
self.p34_star = np.array([[0],[0.8]])
self.p24_star = np.array([[-0.8],[0.8]])
self.p14_star = np.array([[-0.8],[0]])
self.d = np.float32(0.8)
self.r_safe = params.r_safe
self.dd = np.float32(np.sqrt(np.square(self.d)+np.square(self.d)))
self.c = params.c
self.cf = params.cf # new gain for formation control Nelson
self.cI = params.cI
self.cP = params.cP
self.cD = params.cD
self.calpha = params.calpha
self.czeta = params.czeta
self.U_old = np.array([0, 0])
self.U_oldd = np.array([0, 0])
self.k = 0 # current iteration of the Euler method
self.h = params.h # stepsize
self.zeta4_old = np.zeros((2,1))
self.zeta_values = np.empty(shape=(0,0))
self.zeta4 = np.zeros((2,1))
self.gamma_old = np.zeros((2,1)) # sta
self.gamma = np.zeros((2,1))
self.pGoal = params.pGoal
self.pcen_old = np.zeros((2,1))
self.Ug_lim = params.Ug_lim
# Motion parameters
self.x_dot = np.float32(0)
self.y_dot = np.float32(0)
self.r_dot = np.float32(0)
self.mu_x = self.x_dot*np.array([0, -1, 0, -1, 0])
self.mut_x = self.x_dot*np.array([0, 1, 0, 1, 0])
self.mu_y = self.y_dot*np.array([-1, 0, 0, 0, 1])
self.mut_y = self.y_dot*np.array([1, 0, 0, 0, -1])
self.mu_r = self.r_dot*np.array([-1, -1, 0, 1, -1])
self.mut_r = self.r_dot*np.array([1, 1, 0, -1, 1])
self.mu = self.mu_x+self.mu_y+self.mu_r
self.mut = self.mut_x+self.mut_y+self.mut_r
# prepare Log arrays
self.E3_log = np.array([])
self.E4_log = np.array([])
self.E5_log = np.array([])
self.Un = np.float32([])
self.U_log = np.array([])
self.time = np.float64([])
self.time_log = np.array([])
self.now = np.float64([rospy.get_time()])
self.begin = np.float64([rospy.get_time()])
self.l = 0
# prepare shutdown
rospy.on_shutdown(self.shutdown)
# prepare publisher
self.pub_vel = rospy.Publisher('/n_4/cmd_vel', Twist, queue_size=1)
self.velocity = Twist()
self.pub_zeta = rospy.Publisher('/n_4/zeta_values', numpy_msg(Floats), queue_size=1)
self.pub_pcen = rospy.Publisher('/n_4/pcen', numpy_msg(Floats), queue_size=1)
# subscribe to r_values topic
rospy.Subscriber('/n_4/r_values', numpy_msg(Floats), self.controller)
rospy.Subscriber('/n_4/obstacles', numpy_msg(Floats), self.obstacleAvoidance)
# Subscribe to the filtered odometry node
rospy.Subscriber('/n_4/odometry/filtered', Odometry, self.formationMotion)
rospy.Subscriber('/n_1/Ug_cmd_vel', Twist, self.Ug_cmd_vel)
# subscribe to zeta_values topic of each controller
rospy.Subscriber('/n_1/zeta_values', numpy_msg(Floats), self.zeta1_sub)
rospy.Subscriber('/n_2/zeta_values', numpy_msg(Floats), self.zeta2_sub)
rospy.Subscriber('/n_3/zeta_values', numpy_msg(Floats), self.zeta3_sub)
def __init__(self):
self.objectCoord = objCoord()
self.isApriltag_received = False
rospy.logwarn("AprilTag Center Node [ONLINE]...")
# rospy shutdown
rospy.on_shutdown(self.cbShutdown)
# Publish to Bool msg
self.isApriltag_topic = "/isApriltag"
self.isApriltag_sub = rospy.Subscriber(self.isApriltag_topic, Bool,
self.cbIsApriltag)
# Subscribe to apriltagList msg
self.isApriltagN_topic = "/isApriltag/N"
self.isApriltagN_sub = rospy.Subscriber(self.isApriltagN_topic,
apriltagList,
self.cbIsApriltagN)
# Subscribe to apriltagCenter msg
self.apriltagCenterX_topic = "/isApriltag/Center/X"
self.apriltagCenterX_sub = rospy.Subscriber(self.apriltagCenterX_topic,
apriltagCenter,
self.cbIsApriltagCenterX)
# Subscribe to apriltagCenter msg
self.apriltagCenterY_topic = "/isApriltag/Center/Y"
self.apriltagCenterY_sub = rospy.Subscriber(self.apriltagCenterY_topic,
apriltagCenter,
self.cbIsApriltagCenterY)
# Subscribe to CompressedImage msg
self.telloCameraInfo_topic = "/tello/camera/camera_info"
self.telloCameraInfo_sub = rospy.Subscriber(self.telloCameraInfo_topic,
CameraInfo,
self.cbCameraInfo)
# Subscribe to TelloStatus msg
self.telloStatus_topic = "/tello/status"
self.telloStatus_sub = rospy.Subscriber(self.telloStatus_topic,
TelloStatus,
self.cbTelloStatus)
# Subscribe to Odometry msg
self.telloOdom_topic = "/tello/odom"
self.telloOdom_sub = rospy.Subscriber(self.telloOdom_topic, Odometry,
self.cbTelloOdometry)
# Subscribe to PoseWithCovariance msg
self.telloIMU_topic = "/tello/imu"
self.telloIMU_sub = rospy.Subscriber(self.telloIMU_topic, Imu,
self.cbTelloIMU)
# Publish to objCenter msg
self.objCoord_topic = "/isApriltag/objCoord"
self.objCoord_pub = rospy.Publisher(self.objCoord_topic,
objCoord,
queue_size=10)
# Allow up to one second to connection
rospy.sleep(1)
def __init__(self):
rospy.init_node('Arduino', log_level=rospy.DEBUG)
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
self.baud = int(rospy.get_param("~baud", 57600))
self.timeout = rospy.get_param("~timeout", 0.5)
self.base_frame = rospy.get_param("~base_frame", 'base_link')
# Overall loop rate: should be faster than fastest sensor rate
self.rate = int(rospy.get_param("~rate", 50))
r = rospy.Rate(self.rate)
# Rate at which summary SensorState message is published. Individual sensors publish
# at their own rates.
self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
self.use_base_controller = rospy.get_param("~use_base_controller",
False)
# Set up the time for publishing the next SensorState message
now = rospy.Time.now()
self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
self.t_next_sensors = now + self.t_delta_sensors
# Initialize a Twist message
self.cmd_vel = Twist()
# A cmd_vel publisher so we can stop the robot when shutting down
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Initialize the controlller
self.controller = Arduino(self.port, self.baud, self.timeout)
# Make the connection
self.controller.connect()
rospy.loginfo("Connected to Arduino on port " + self.port + " at " +
str(self.baud) + " baud")
# get arduino version
self.year = 0
self.date = 0
self.year, self.date = self.controller.get_arduino_version()
rospy.loginfo("arduino version is " + str(self.year) + str(self.date))
# Reserve a thread lock
mutex = thread.allocate_lock()
# Initialize the base controller if used
if self.use_base_controller:
self.myBaseController = BaseController(self.controller,
self.base_frame)
# Start polling the sensors and base controller
while not rospy.is_shutdown():
if self.use_base_controller:
mutex.acquire()
self.myBaseController.poll()
mutex.release()
r.sleep()
def __init__(self):
rospy.init_node('Follow')
rospy.on_shutdown(self.shutdown)
rospy.logerr('Let\'s go')
self.trace = Concurrence(outcomes=['succeeded', 'aborted'],
default_outcome='aborted',
outcome_map={
'succeeded': {
'STOP': 'stop'
},
'aborted': {
'FOLLOW': 'aborted'
},
'aborted': {
'FOLLOW': 'preempted'
}
},
child_termination_cb=self.child_cb,
input_keys=['people_id'])
with self.trace:
self.meta_follow = StateMachine(
['succeeded', 'aborted', 'preempted'],
input_keys=['people_id'])
with self.meta_follow:
self.meta_follow.userdata.pos_xm = Pose()
self.meta_follow.userdata.rec = 0.2
StateMachine.add('FIND',
LegTracker().tracker,
transitions={
'invalid': 'WAIT',
'valid': 'FIND',
'preempted': 'preempted'
},
remapping={
'pos_xm': 'pos_xm',
'people_id': 'people_id'
})
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'META_NAV',
'error': 'META_NAV'
})
self.meta_nav = Concurrence(
outcomes=['time_over', 'get_pos', 'aborted'],
default_outcome='aborted',
outcome_map={
'time_over': {
'WAIT': 'succeeded'
},
'get_pos': {
'NAV': 'succeeded'
},
'aborted': {
'NAV': 'aborted'
}
},
child_termination_cb=self.nav_child_cb,
input_keys=['pos_xm'])
with self.meta_nav:
Concurrence.add('NAV',
NavStack(),
remapping={'pos_xm': 'pos_xm'})
Concurrence.add('WAIT', Wait_trace())
StateMachine.add('META_NAV',
self.meta_nav,
transitions={
'get_pos': 'FIND',
'time_over': 'FIND',
'aborted': 'FIND'
})
Concurrence.add('FOLLOW', self.meta_follow)
Concurrence.add('STOP', CheckStop())
self.sm_GPSR = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.sm_GPSR:
self.sm_GPSR.userdata.target = list()
self.sm_GPSR.userdata.action = list()
self.sm_GPSR.userdata.task_num = 0
self.sm_GPSR.userdata.current_task = -1
self.sm_GPSR.userdata.people_id = -1
# StateMachine.add('RUNNODE',RunNode(),
# transitions={'succeeded':'XM','aborted':'aborted'})
StateMachine.add(
'XM',
self.trace,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
#'preempted':'error'
})
intro_server = IntrospectionServer('sm_gpsr', self.sm_GPSR, '/SM_ROOT')
intro_server.start()
out = self.sm_GPSR.execute()
print out
intro_server.stop()
self.smach_bool = True
def main():
def state_attrdict(uav_state_msg):
e = AttrDict()
e.ac_id = uav_state_msg.ac_id
e.time = uav_state_msg.header.stamp.to_sec()
e.recv_time = rospy.get_rostime().to_sec()
e.position = (uav_state_msg.position.x, uav_state_msg.position.y,
uav_state_msg.position.z)
e.attitude = (uav_state_msg.attitude.phi, uav_state_msg.attitude.psi,
uav_state_msg.attitude.theta)
return e
def compute_stats(state, expected_state):
"""Compute errors between corresponding UAV states.
Arguments:
state:
expected_state:
Rerturns:
skyscanner.msg.UAVStats object.
"""
def angle_diff(a, b):
"""Calculate the distance between two angles in degrees.
Arguments:
a: measured angle in degrees
b: reference angle in degrees
Returns:
Distance between a and b including sign
"""
phi = np.abs(a - b) % 360
sign = 1
if not ((a - b >= 0 and a - b <= 180) or
(a - b <= -180 and a - b >= -360)):
sign = -1
if phi > 180:
result = 360 - phi
else:
result = phi
return result * sign
stats = UAVStats()
stats.header = Header(stamp=rospy.Time(state.time))
stats.ac_id = state.ac_id
stats.heading_error = \
angle_diff(state.attitude[1]*np.pi/180,
expected_state.attitude[1]*np.pi/180) * 180/np.pi
stats.altitude_error = state.position[2] - expected_state.position[2]
p1 = np.array(state.position[:2])
p2 = np.array(expected_state.position[:2])
stats.crosstrack_error = np.linalg.norm(p1 - p2)
return stats
def get_until(queue, condition):
"""Generator returning all elements in queue until condition is met.
Arguments:
queue: Queue.queue
condition: fn(e) -> True or False where `e` element of queue.
Must return True while condition is being met, False otherwise.
"""
try:
e = queue.get(False)
while condition(e):
yield e
e = queue.get(False)
else:
with queue.mutex:
queue.queue.appendleft(e)
except Empty:
return
def on_state_recv(state):
"""Handle state reception."""
ac_id = state.ac_id
if ac_id not in ac:
rospy.logwarn("[stats] ac_id '{}' not recognized".format(ac_id))
return
if USE_MATH_TRAJ:
publish_stats_math_traj(state)
else:
publish_stats_exp_pos(state)
# else:
def publish_stats_math_traj(state):
global actual_traj
msg_time = state.header.stamp.to_sec()
cond = lambda e: msg_time < e.time_limit
for s in get_until(ac[ac_id].traj_q, cond):
actual_traj = s
if actual_traj is None:
return
np_pos = np.array(
[state.position.x, state.position.y, state.position.z])
stats = UAVStats()
stats.header = Header(stamp=rospy.Time(msg_time))
stats.ac_id = state.ac_id
stats.heading_error = 0
stats.altitude_error = 0
stats.crosstrack_error = actual_traj.crosstrack_error(np_pos)[1]
rospy.loginfo(stats)
ac[ac_id].stats_pub.publish(stats)
def publish_stats_exp_pos(state):
msg_time = state.header.stamp.to_sec()
cond = lambda e: np.around(msg_time - e.time, decimals=DECIMAL_DIGITS
) >= 0
# Get first element meeting condition discarding the previous ones
s = None
for s in get_until(ac[ac_id].expected_states_q, cond):
pass
if s is None:
return
if s.time != msg_time:
rospy.logwarn("[stats] s.time != msg_time '{}!={}'".format(
s.time, msg_time))
stats = compute_stats(state_attrdict(state), s)
rospy.loginfo(stats)
ac[ac_id].stats_pub.publish(stats)
def on_expected_state_recv(exp_state):
"""Handle expected state reception."""
ac_id = exp_state.ac_id
if ac_id in ac:
ac[ac_id].expected_states_q.put(state_attrdict(exp_state))
else:
rospy.logwarn("[stats] ac_id '{}' not recognized".format(ac_id))
def on_trajectory_recv(trajectory_seq):
"""Handle TrajectorySequence message reception."""
print("trajectory recv")
ac_id = trajectory_seq.ac_id
if ac_id in ac:
for trajectory in trajectory_seq.trajectories:
if trajectory.circle is True:
cir = Circumference(np.array([
trajectory.origin.x, trajectory.origin.y,
trajectory.origin.z
]),
-trajectory.radius,
timeout=trajectory.duration,
time_limit=trajectory.time_limit)
ac[ac_id].traj_q.put(cir)
else:
lin = Segment(np.array([
trajectory.origin.x, trajectory.origin.y,
trajectory.origin.z
]),
np.array([
trajectory.destination.x,
trajectory.destination.y,
trajectory.destination.z
]),
timeout=trajectory.duration,
time_limit=trajectory.time_limit)
ac[ac_id].traj_q.put(lin)
else:
rospy.logwarn("[stats] ac_id '{}' not recognized".format(ac_id))
def on_shutdown():
pass
rospy.init_node('stats')
rospy.on_shutdown(on_shutdown)
# Setup math precision
global ABS_TOL, DECIMAL_DIGITS, actual_traj
ABS_TOL = rospy.get_param(rospy.search_param('abs_tol'), 1e-3)
DECIMAL_DIGITS = int(np.around(np.log10(1. / ABS_TOL)))
# rospy.Subscriber('/tick', Clock, on_tick_recv, queue_size=10)
# Get aircraft namespaces
aircraft_ns = rospy.get_param('aircrafts_ns')
ac = {}
for ns in aircraft_ns:
ac_id = rospy.get_param(rospy.search_param('/'.join([ns, 'ac_id'])))
ac[ac_id] = AttrDict()
ac[ac_id].name = ns
ac[ac_id].expected_states_q = Queue()
ac[ac_id].traj_q = Queue()
ac[ac_id].state_sub = rospy.Subscriber('/'.join([ns, 'uav_state']),
UAVState,
on_state_recv,
queue_size=1)
ac[ac_id].exp_state_sub = rospy.Subscriber('/'.join(
[ns, 'expected_uav_state']),
UAVState,
on_expected_state_recv,
queue_size=1)
ac[ac_id].exp_state_sub = rospy.Subscriber('/'.join(
[ns, 'trajectory_sequence']),
TrajectorySequence,
on_trajectory_recv,
queue_size=1)
ac[ac_id].stats_pub = rospy.Publisher('/'.join([ns, 'stats']),
UAVStats,
queue_size=10)
rospy.spin()
def __init__(self):
rospy.init_node('nav_test', anonymous=False)
rospy.on_shutdown(self.shutdown)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
goal = MoveBaseGoal()
quaternion = Quaternion()
while 1:
#goal 1
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = 0.0
quaternion.w = 1.0
goal.target_pose.pose.position.x = 2.0
goal.target_pose.pose.position.y = -0.1
goal.target_pose.pose.position.z = 0
goal.target_pose.pose.orientation = quaternion
self.move_base.send_goal(goal)
# Allow 1 minute to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(600))
# If we don't get there in time, abort the goal
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
rospy.sleep(5)
#goal 2
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = 0.0
quaternion.w = 1.0
goal.target_pose.pose.position.x = 4.0
goal.target_pose.pose.position.y = -0.1
goal.target_pose.pose.position.z = 0
goal.target_pose.pose.orientation = quaternion
self.move_base.send_goal(goal)
# Allow 1 minute to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(600))
# If we don't get there in time, abort the goal
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
rospy.sleep(5)
def __init__(self):
rospy.init_node("ur5_vision", anonymous=False)
self.bridge = cv_bridge.CvBridge()
self.image_sub = rospy.Subscriber('/ur5/usbcam/image_raw', Image, self.image_callback)
self.cxy_pub = rospy.Publisher('cxy', Twist, queue_size=10)
rospy.loginfo("Starting node moveit_cartesian_path")
rospy.on_shutdown(self.cleanup)
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the move group for the ur5_arm
self.arm = moveit_commander.MoveGroupCommander('manipulator')
# Get the name of the end-effector link
end_effector_link = self.arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = "/base_link"
# Set the ur5_arm reference frame accordingly
self.arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
self.arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.1)
# Get the current pose so we can add it as a waypoint
start_pose = self.arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints= []
# Set the first waypoint to be the starting pose
# Append the pose to the waypoints list
wpose = deepcopy(start_pose)
# Set the next waypoint to the right 0.5 meters
wpose.position.x = -0.2
wpose.position.y = -0.2
wpose.position.z = 0.3
waypoints.append(deepcopy(wpose))
wpose.position.x = 0.1052
wpose.position.y = -0.4271
wpose.position.z = 0.4005
wpose.orientation.x = 0.4811
wpose.orientation.y = 0.4994
wpose.orientation.z = -0.5121
wpose.orientation.w = 0.5069
waypoints.append(deepcopy(wpose))
if np.sqrt((wpose.position.x-start_pose.position.x)**2+(wpose.position.x-start_pose.position.x)**2 \
+(wpose.position.x-start_pose.position.x)**2)<0.1:
rospy.loginfo("Warnig: target position overlaps with the initial position!")
# self.arm.set_pose_target(wpose)
# Set the internal state to the current state
self.arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
"""moveit_commander.move_group.MoveGroupCommander.compute_cartesian_path(
self, waypoints, eef_step, jump_threshold, avoid_collisios= True)
Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the
poses specified as waypoints. Configurations are computed for every eef_step meters;
The jump_threshold specifies the maximum distance in configuration space between consecutive points
in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed,
the actual RobotTrajectory.
"""
plan, fraction = self.arm.compute_cartesian_path(waypoints, 0.01, 0.0, True)
# plan = self.arm.plan()
# If we have a complete plan, execute the trajectory
if 1-fraction < 0.2:
rospy.loginfo("Path computed successfully. Moving the arm.")
num_pts = len(plan.joint_trajectory.points)
rospy.loginfo("\n# intermediate waypoints = "+str(num_pts))
self.arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed")
rospy.loginfo('line_detector_node processing first image.')
self.nprocessed += 1
def info(self):
delta = time.time() - self.t0
if self.nreceived:
skipped_perc = (100.0 * self.nskipped / self.nreceived)
else:
skipped_perc = 0
def fps(x):
return '%.1f fps' % (x / delta)
m = ('In the last %.1f s: received %d (%s) processed %d (%s) skipped %d (%s) (%1.f%%)' %
(delta, self.nreceived, fps(self.nreceived),
self.nprocessed, fps(self.nprocessed),
self.nskipped, fps(self.nskipped), skipped_perc))
return m
if __name__ == '__main__':
rospy.init_node('line_detector',anonymous=False)
line_detector_node = LineDetectorNode()
rospy.on_shutdown(line_detector_node.onShutdown)
rospy.spin()
self.time = IntegrationTime()
self.find_threshold = rospy.ServiceProxy('/desaturator/find_threshold',
Empty)
self.save_srv = rospy.Service('~save', Empty, self.save_cb)
self.data = open('calibration.dat', 'a')
#self.timer = rospy.Timer(rospy.Duration(5), self.timer_cb)
def hist_cb(self, msg):
for b, l in zip(msg.bins, msg.limits):
if b > 10:
self.current = (l, self.time.get())
return
#def timer_cb(self, event):
def save_cb(self, req):
rospy.loginfo('Finding threshold...')
self.find_threshold()
rospy.loginfo('Done, saving %.3f -- %i' % (self.current))
self.data.write('%.3f %i\n' % self.current)
return []
def shutdown(self):
self.data.close()
if __name__ == '__main__':
rospy.init_node(NODE)
sdn = StoreDataNode()
rospy.on_shutdown(lambda: sdn.shutdown())
rospy.spin()
def __init__(self):
# Initialize the node
# rospy.init_node('my_node_name', anonymous=True)#
# The anonymous keyword argument is mainly used for nodes where
# you normally expect many of them to be running and
# don't care about their names (e.g. tools, GUIs)
#see http://wiki.ros.org/rospy/Overview/Initialization%20and%20Shutdown
rospy.init_node('doitall_node', anonymous=False)
# tell user how to stop TurtleBot and print on Terminal
rospy.loginfo("To stop Jakcal CTRL + C")
# What function to call when you ctrl + c
#rospy.on_shutdown(h)
#Register handler to be called when rospy process begins shutdown.
#h is a function that takes no arguments.
#see http://wiki.ros.org/rospy/Overview/Initialization%20and%20Shutdown
rospy.on_shutdown(self.shutdown)
self.f1 = open('jackal_ground_truth_coordinates.txt', 'w+')
self.f2 = open('jackal_amcl_coordinates.txt', 'w+')
#with open('jackal_ground_truth_coordinates.txt', 'r+') as self.f1: pass
#with open('jackal_amcl_coordinates.txt', 'r+') as self.f2: pass
# Subscribe to the amcl_pose topic and set
# the appropriate callbacks
# see http://answers.ros.org/question/49282/amcl-and-laser_scan_matcher/
# see http://answers.ros.org/question/185907/help-with-some-code-send-a-navigation-goal/
#self.link_states = rospy.Subscriber('/gazebo/link_states', LinkStates, self.link_stateCallback)
# Subscribe to the amcl_pose topic and set
# the appropriate callbacks
# see http://answers.ros.org/question/49282/amcl-and-laser_scan_matcher/
# see http://answers.ros.org/question/185907/help-with-some-code-send-a-navigation-goal/
self.model_states = rospy.Subscriber('/gazebo/model_states',
ModelStates,
self.model_stateCallback)
# Subscribe to the amcl_pose topic and set
# the appropriate callbacks
# see http://answers.ros.org/question/49282/amcl-and-laser_scan_matcher/
# see http://answers.ros.org/question/185907/help-with-some-code-send-a-navigation-goal/
self.amcl_sub = rospy.Subscriber('/amcl_pose',
PoseWithCovarianceStamped,
self.amclPoseCallback)
# Create publisher for cmd_vel topic
# cmd_vel publisher can "talk" to Jackal and tell it to move
self.cmd_vel = rospy.Publisher('/jackal_velocity_controller/cmd_vel',
Twist,
queue_size=10)
# Create publisher for amcl initial pose
# set the initial pose and covariance
self.amcl_initialpose_pub = rospy.Publisher('/initialpose',
PoseWithCovarianceStamped,
queue_size=10)
#initialize coordinates list
self.jackal_amcl_coordinates = []
self.jackal_amcl_timestamps = []
#initialize coordinates list
self.jackal_coordinates = []
self.jackal_timestamps = []
#injecing initial pose
try:
self.inject_initial_pose = sys.argv[3]
except:
self.inject_initial_pose = 'False'
# Publishing rate = 100 Hz look for odometry
#for sleeping and rate see
#http://wiki.ros.org/rospy/Overview/Time
self.rate = rospy.Rate(20)
detect_dict = {}
# We consider that when there is a big Z axis component there has been a very bif front crash
detection_dict = {
"front": (message == "front" or message == "up"
or message == "down"),
"left": message == "left",
"right": message == "right",
"back": message == "back"
}
return detection_dict
if __name__ == "__main__":
rospy.init_node('imu_topic_subscriber', log_level=rospy.INFO)
imu_reader_object = ImuTopicReader()
rospy.loginfo(imu_reader_object.get_imu_data())
rate = rospy.Rate(0.5) # Twice per second
ctrl_c = False
def shutdownhook():
# WORKS BETTER THAN THE rospy.is_shut_down()
global ctrl_c
print "shutdown time!"
ctrl_c = True
rospy.on_shutdown(shutdownhook)
while not ctrl_c:
data = imu_reader_object.get_imu_data()
rospy.loginfo(data)
rate.sleep()
def __init__(self):
# 设置rospy在终止节点时执行的关闭函数
rospy.on_shutdown(self.shutdown)
# 获取参数
self.goal_distance = rospy.get_param("~goal_distance", 1.0) # meters
self.goal_angle = radians(rospy.get_param(
"~goal_angle", 90)) # degrees converted to radians
self.linear_speed = rospy.get_param("~linear_speed",
0.2) # meters per second
self.angular_speed = rospy.get_param("~angular_speed",
0.7) # radians per second
self.angular_tolerance = radians(
rospy.get_param("~angular_tolerance", 2)) # degrees to radians
self.velocity_topic = rospy.get_param(
'~vel_topic', '/mobile_base/mobile_base_controller/cmd_vel')
self.rate_pub = rospy.get_param('~velocity_pub_rate', 20)
self.rate = rospy.Rate(self.rate_pub)
# 初始化发布速度的topic
self.cmd_vel = rospy.Publisher(self.velocity_topic,
Twist,
queue_size=1)
self.base_frame = rospy.get_param('~base_frame',
'/base_link') #set the base_frame
self.odom_frame = rospy.get_param('~odom_frame',
'/odom') #set the odom_frame
# 初始化tf
self.tf_listener = tf.TransformListener()
rospy.sleep(2)
try:
self.tf_listener.waitForTransform(self.base_frame, self.odom_frame,
rospy.Time(0),
rospy.Duration(5.0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logerr(
'tf catch exception when robot waiting for transform......')
exit(-1)
# 循环四次画出正方形
for i in range(4):
self.Linear_motion()
# 再进行旋转之前时机器人停止
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
self.Rotational_motion()
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
#结束时使机器人停止
self.cmd_vel.publish(Twist())
