def call_service(self):
rospy.wait_for_service('/zeroconf/add_listener')
try:
add_listener = rospy.ServiceProxy('/zeroconf/add_listener', AddListener)
r = add_listener('_workstation._tcp')
except rospy.ServiceException, e:
print "Servce call failed: %s"%e
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):
# ROS initialization:
rospy.init_node('pose_manager')
self.poseLibrary = dict()
self.readInPoses()
self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
execute_cb=self.executeBodyPose,
auto_start=False)
self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
try:
rospy.wait_for_service("stop_walk_srv", timeout=2.0)
self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
except:
rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
+"This is normal if there is no nao_walker running.")
self.stopWalkSrv = None
self.poseServer.start()
rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());
else:
rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
rospy.signal_shutdown("Required component missing");
def test_basic_localization(self):
global_localization = int(sys.argv[1])
self.target_x = float(sys.argv[2])
self.target_y = float(sys.argv[3])
self.target_a = float(sys.argv[4])
tolerance_d = float(sys.argv[5])
tolerance_a = float(sys.argv[6])
target_time = float(sys.argv[7])
if global_localization == 1:
#print 'Waiting for service global_localization'
rospy.wait_for_service('global_localization')
global_localization = rospy.ServiceProxy('global_localization', Empty)
resp = global_localization()
rospy.init_node('test', anonymous=True)
while(rospy.rostime.get_time() == 0.0):
#print 'Waiting for initial time publication'
time.sleep(0.1)
start_time = rospy.rostime.get_time()
# TODO: This should be replace by a pytf listener
rospy.Subscriber('/tf', tfMessage, self.tf_cb)
while (rospy.rostime.get_time() - start_time) < target_time:
#print 'Waiting for end time %.6f (current: %.6f)'%(target_time,(rospy.rostime.get_time() - start_time))
time.sleep(0.1)
(a_curr, a_diff) = self.compute_angle_diff()
print 'Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr)
print 'Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a)
print 'Diff:\t %16.6f %16.6f %16.6f' % (abs(self.tf.translation.x-self.target_x),abs(self.tf.translation.y - self.target_y), a_diff)
self.assertNotEquals(self.tf, None)
self.assertTrue(abs(self.tf.translation.x - self.target_x) <= tolerance_d)
self.assertTrue(abs(self.tf.translation.y - self.target_y) <= tolerance_d)
self.assertTrue(a_diff <= tolerance_a)
def _setup_ik(self):
'''Sets up services for inverse kinematics'''
ik_info_srv_name = ('pr2_' + self._side() +
'_arm_kinematics_simple/get_ik_solver_info')
ik_srv_name = 'pr2_' + self._side() + '_arm_kinematics_simple/get_ik'
rospy.wait_for_service(ik_info_srv_name)
ik_info_srv = rospy.ServiceProxy(ik_info_srv_name,
GetKinematicSolverInfo)
solver_info = ik_info_srv()
rospy.loginfo('IK info service has responded for '
+ self._side() + ' arm.')
rospy.wait_for_service(ik_srv_name)
self.ik_srv = rospy.ServiceProxy(ik_srv_name,
GetPositionIK, persistent=True)
rospy.loginfo('IK service has responded for ' + self._side() + ' arm.')
# Set up common parts of an IK request
self.ik_request = GetPositionIKRequest()
self.ik_request.timeout = rospy.Duration(4.0)
self.ik_joints = solver_info.kinematic_solver_info.joint_names
self.ik_limits = solver_info.kinematic_solver_info.limits
ik_links = solver_info.kinematic_solver_info.link_names
request = self.ik_request.ik_request
request.ik_link_name = ik_links[0]
request.pose_stamped.header.frame_id = 'base_link'
request.ik_seed_state.joint_state.name = self.ik_joints
request.ik_seed_state.joint_state.position = [0] * len(self.ik_joints)
def turtlerun():
#T = input('PLease enter the number of T:')
pub = rospy.Publisher('/turtlesim1/turtle1/cmd_vel',Twist, queue_size=10)
rate = rospy.Rate(50)
rospy.wait_for_service('/turtlesim1/turtle1/teleport_absolute')
start = rospy.ServiceProxy('/turtlesim1/turtle1/teleport_absolute',TeleportAbsolute)
start(5.5,5.5,0.5)
T = rospy.get_param("turtlesim1/T")
t0 = rospy.get_time()
while not rospy.is_shutdown():
now_time = rospy.get_time()
t = now_time-t0
x = 3*sin((4*pi*t)/T)
y = 3*sin((2*pi*t)/T)
dx = 12*pi*cos((4.0*pi*t)/T)/T
dy = 6*pi*cos((2.0*pi*t)/T)/T
v = sqrt(dx*dx+dy*dy)
dx2 = -48*pi*pi*sin((4*pi*t)/T)/(T*T)
dy2 = -12*pi*pi*sin((2*pi*t)/T)/(T*T)
w = ((dx*dy2)-(dy*dx2)) /((dx*dx) + (dy*dy))
turtlemove = Twist(Vector3(v,0,0),Vector3(0,0,w))
rospy.loginfo(turtlemove)
pub.publish(turtlemove)
rate.sleep()
def _threadedCall(self, msg):
rosMsg = rospy.AnyMsg()
rosMsg._buff = msg
rospy.wait_for_service(self._addr[1], timeout=5)
serviceFunc = rospy.ServiceProxy(self._addr[1], self._srvCls)
return serviceFunc(rosMsg)
def talker():
rospy.init_node('random_motors', anonymous=False)
r = rospy.Rate(0.5) # 10hz
joints = [rospy.get_param("~pan_name", 'pan_joint'), rospy.get_param("~tilt_name", 'tilt_joint')]
torque_enable = dict()
servo_position = dict()
#Configure joints
for controller in sorted(joints):
try:
rospy.loginfo("controller is "+controller)
# Torque enable/disable control for each servo
torque_service = '/' + controller + '/torque_enable'
rospy.wait_for_service(torque_service)
torque_enable[controller] = rospy.ServiceProxy(torque_service, TorqueEnable)
# Start each servo in the disabled state so we can move them by hand
torque_enable[controller](False)
# The position controllers
servo_position[controller] = rospy.Publisher('/' + controller + '/command', Float64)
except:
rospy.logerr("Can't contact servo services!")
while not rospy.is_shutdown():
for controller in sorted(joints):
rand_val = uniform(-2,2)
#Send random float to each joint
servo_position[controller].publish(rand_val)
r.sleep()
"""
def verify_boards(self, check):
if self.has_finished:
return False
try:
rospy.wait_for_service('mcb_conf_results', 15)
except:
msg = "MCB conf results service not available. Unable to configure MCB's"
rospy.logerr(msg)
self.finished(False, msg)
return False
if not self.count_boards():
return False
if not self.check_link():
return False
if not self.get_serials():
return False
if check:
return self.check_boards()
return True
def test():
rospy.init_node('test')
loginfo("Initialized node Controller")
rospy.wait_for_service("/move_end_effector_trajectory")
joint_action_server = rospy.ServiceProxy("/move_end_effector_trajectory", JointAction)
tool_trajectory = rospy.ServiceProxy("/move_tool_trajectory", JointAction)
loginfo("Initialized Joint Action Server Proxy")
rospy.wait_for_service("/end_effector_position")
position_server = rospy.ServiceProxy("/end_effector_position", EndEffectorPosition)
loginfo("Initialized position server proxy")
# rospy.sleep(5.0)
loginfo("Making position call")
initial_position = position_server().position
loginfo(initial_position)
joint_action_server([5.0, 10.0],
[Vector3(initial_position.x + 0.1*np.random.rand(),
initial_position.y + 0.1*np.random.rand(),
initial_position.z + 0.1*np.random.rand()),
initial_position],
[Vector3(0.11, 0.0, 0.0),
Vector3(0.0, 0.0, 0.0)])
loginfo(position_server().position.x - initial_position.x)
loginfo(position_server().position.y - initial_position.y)
loginfo(position_server().position.z - initial_position.z)
def send_results(self, test_result):
if self.data_sent:
return
rospy.wait_for_service('/test_result', 15)
self.result_service.call(test_result)
self.data_sent = True
def reset_iris_neutral_value(self):
if self.dic_sequence_services['checked_sequence_of_missions'] == True:
new_service = {}
trigger_instant = self._widget.TriggerInstantNeutral.value()
new_service['trigger_instant'] = trigger_instant
self.dic_sequence_services['last_trigger_time'] = trigger_instant
new_service['service_name'] = 'IrisPlusResetNeutral'
new_service['inputs_service'] = {}
self.dic_sequence_services['list_sequence_services'].append(new_service)
else:
try:
# time out of one second for waiting for service
rospy.wait_for_service(self.namespace+'IrisPlusResetNeutral',1.0)
try:
# request reseting neutral value for iris+ (implicit, with keywords)
ResetingNeutral = rospy.ServiceProxy(self.namespace+'IrisPlusResetNeutral', IrisPlusResetNeutral)
reply = ResetingNeutral()
if reply.received == True:
self._widget.ThrottleNeutralValue.setValue(reply.k_trottle_neutral)
rospy.logwarn('Service for reseting neutral value succeded')
except rospy.ServiceException as exc:
rospy.logwarn("Service did not process request: " + str(exc))
rospy.logwarn('Service for reseting neutral value failed')
pass
except rospy.ServiceException as exc:
rospy.logwarn("Service did not process request: " + str(exc))
rospy.logwarn('Service for reseting neutral value failed')
pass
def moveBBSrv(req):
try:
rospy.wait_for_service('scan_base_pose',10)
except rospy.ROSException, e:
print "Service not available: %s"%e
outcome_detectObjectSrv = 'failed'
def calltrig(name):
rospy.wait_for_service(name)
try:
s = rospy.ServiceProxy(name,std_srvs.srv.Trigger)
resp = s()
except rospy.ServiceException,e:
print "Failed: %s" % e
def send_list_of_services(self):
# # debug
# for service in self.dic_sequence_services['list_sequence_services']:
# print(service)
# print('\n\n')
# request service
try:
# time out of one second for waiting for service
rospy.wait_for_service(self.namespace+'ServiceSequencer',1.0)
try:
request = rospy.ServiceProxy(self.namespace+'ServiceSequencer', ServiceSequence)
service_sequence = json.dumps(self.dic_sequence_services['list_sequence_services'])
reply = request(service_sequence = service_sequence)
if reply.received == True:
# if controller receives message
print('Success')
except rospy.ServiceException as exc:
rospy.logwarn("Service did not process request: " + str(exc))
except rospy.ServiceException as exc:
rospy.logwarn("Service did not process request: " + str(exc))
def ResetSimulator(self):
try:
# time out of one second for waiting for service
rospy.wait_for_service(self.namespace+'ResetSimulator',1.0)
try:
AskForStart = rospy.ServiceProxy(self.namespace+'ResetSimulator', StartSim)
# if button is pressed save data
if self._widget.ButtonSTART_SIM.isChecked():
# request controller to save data
reply = AskForStart(True)
if reply.Started == True:
# if controller receives message, we know it
# print('Reseted')
self._widget.SimulatorSuccess.setChecked(True)
self._widget.SimulatorFailure.setChecked(False)
else:
self._widget.SimulatorSuccess.setChecked(False)
self._widget.SimulatorFailure.setChecked(True)
# else: do nothing
except rospy.ServiceException:
# print "Service call failed: %s"%e
self._widget.SimulatorSuccess.setChecked(False)
self._widget.SimulatorFailure.setChecked(True)
except:
# print "Service not available ..."
self._widget.SimulatorSuccess.setChecked(False)
self._widget.SimulatorFailure.setChecked(True)
pass
def get_path(self, request):
print "Waiting for path service"
rospy.wait_for_service('/ogmpp_path_planners/plan')
print "Service ok"
try:
pathSrv = rospy.ServiceProxy('/ogmpp_path_planners/plan', OgmppPathPlanningSrv)
path = OgmppPathPlanningSrvRequest()
path.method = "uniform_prm"
path.data.begin.x = self.robot_x
path.data.begin.y = self.robot_y
path.data.end.x = request.x
path.data.end.y = request.y
pathRes = pathSrv(path)
#~ print pathRes
ros_path = Path()
ros_path.header.frame_id = "map"
_map = OccupancyGrid()
for p in self.path:
ps = PoseStamped()
ps.header.frame_id = "map"
ps.pose.position.x = p[0] * _map.data.info.resolution + \
_map.data.info.origin.position.x
ps.pose.position.y = p[1] * _map.data.info.resolution + \
_map.data.info.origin.position.y
print ps
ros_path.poses.append(ps)
#~ self.path_publisher.publish(ros_path)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def update_kalman(ar_tags):
print "started update"
rospy.wait_for_service('innovation')
update = rospy.ServiceProxy('innovation', NuSrv)
listener = tf.TransformListener()
while True:
try:
try:
(trans, rot) = listener.lookupTransform(ar_tags['arZ'], ar_tags['ar1'], rospy.Time(0))
except:
print "Couldn't look up transform"
continue
lin = Vector3()
quat = Quaternion()
lin.x = trans[0]
lin.y = trans[1]
lin.z = trans[2]
quat.x = rot[0]
quat.y = rot[1]
quat.z = rot[2]
quat.w = rot[3]
transform = Transform()
transform.translation = lin
transform.rotation = quat
test = update(transform, ar_tags['ar1'])
print "Service call succeeded"
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def UnArming_Quad(self,base_name=""):
#This function is used to arm the quad."""
#Un-Arming the Quad
srv_path = self.namespace+'mavros/cmd/arming'
# if base_name!="":
# srv_path = "/%s/%s"%(base_name,srv_path)
try:
rospy.logwarn('Un-Arming Quad ...')
rospy.wait_for_service(srv_path,2.0)
try:
arming = rospy.ServiceProxy(srv_path,CommandBool)
arming_result=arming(False)
if arming_result.success:
rospy.logwarn('Quad is Un-Armed!!!!')
# return True
else:
rospy.logwarn('Cannot Un-arm quad')
# return False
except:
rospy.logwarn('Cannot Un-arm quad')
# return False
except:
rospy.logwarn('No connection to Mavros')
def recoveryAction(self, error):
if error == ErrorLevel.INFORMATIVE:
rospy.loginfo("%s: recovery action %s: INFORMATIVE", self.name, error)
elif error == ErrorLevel.ASK_AUTHORIZATION:
# TODO: Unused!!!
rospy.loginfo("%s: recovery action %s: ASK_AUTHORIZATION", self.name, error)
elif error == ErrorLevel.ABORT_MISSION:
self.abortMission()
elif error == ErrorLevel.CONTROLLED_SURFACE:
self.controlledSurface()
elif error == ErrorLevel.SURFACE:
self.surface()
elif error == ErrorLevel.EMERGENCY_SURFACE:
self.emergencySurface()
elif error == ErrorLevel.DISABLE_PAYLOAD:
try:
rospy.wait_for_service("digital_output", 5)
digital_out_service = rospy.ServiceProxy("digital_output", DigitalOutput)
digital_output = DigitalOutputRequest()
digital_output.digital_out = 3
digital_output.value = False
digital_out_service(digital_output)
except rospy.exceptions.ROSException:
rospy.logerr("%s, Error disabling payload.", self.name)
else:
rospy.loginfo("%s: recovery action %s: INVALID ERROR CODE", self.name, error)
def ik_test(limb, point, orient):
rospy.init_node("rsdk_ik_service_client")
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
poses = {
'left': PoseStamped(
header=hdr,
pose=Pose(
position= point,
orientation=orient
),
),
'right': PoseStamped(
header=hdr,
pose=Pose(
position=point,
orientation=orient
),
),
}
ikreq.pose_stamp.append(poses[limb])
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return 1
def __init__(self):
rospy.init_node("max_sonar")
self.sensor = maxSonar()
# start channel broadcast using SetupAnalogIn
rospy.wait_for_service('arbotix/SetupAnalogIn')
analog_srv = rospy.ServiceProxy('arbotix/SetupAnalogIn', SetupChannel)
req = SetupChannelRequest()
req.topic_name = rospy.get_param("~name")
req.pin = rospy.get_param("~pin")
req.rate = int(rospy.get_param("~rate",10))
analog_srv(req)
# setup a range message to use
self.msg = Range()
self.msg.radiation_type = self.sensor.radiation_type
self.msg.field_of_view = self.sensor.field_of_view
self.msg.min_range = self.sensor.min_range
self.msg.max_range = self.sensor.max_range
# publish/subscribe
self.pub = rospy.Publisher("sonar_range", Range)
rospy.Subscriber("arbotix/"+req.topic_name, Analog, self.readingCb)
rospy.spin()
def __init__(self):
self._position_controller_nav_frame = rospy.get_param('~position_controller_nav_frame', 'odom')
self._current_pose = None
self._robot_frame = rospy.get_param('~robot_frame', 'base_link')
self._next_target_tf_name = rospy.get_param('~target_tf_name', 'path_follower_next_target')
self._pose_threshold_d = rospy.get_param('~pose_threshold_d', 0.2)
self._pose_threshold_r = rospy.get_param('~pose_threshold_r', 0.2)
rospy.loginfo('create TransformListener')
self._tf_listener = tf.TransformListener()
rospy.loginfo('create TransformBroadcaster')
self._tf_broadcaster = tf.TransformBroadcaster()
set_pose_name = rospy.get_param('~set_pose', '/position_controller/set_target_pos')
rospy.loginfo('waiting for %s...' % set_pose_name)
rospy.wait_for_service(set_pose_name)
self._set_pose = rospy.ServiceProxy(set_pose_name, SetPos)
self._set_pose_thread = None
# for debug
self._path_pub = rospy.Publisher('path_follower_path', Path, queue_size=10)
action_name = rospy.get_param('~action_name', '/follow_path')
rospy.loginfo('create action server %s', action_name)
self.server = actionlib.SimpleActionServer(action_name, FollowPathAction, self.execute, False)
self.server.start()
def status(self):
rospy.wait_for_service('/qbo_video_record/status')
try:
status = rospy.ServiceProxy('/qbo_video_record/status', StatusRecord)
resStatus = status()
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def __init__(self):
rospy.wait_for_service("static_map")
static_map = rospy.ServiceProxy("static_map", GetMap)
try:
self.map = static_map().map
except:
print "error receiving map"
def SaveDataClient(self):
try:
# time out of one second for waiting for service
rospy.wait_for_service(self.namespace+'SaveDataFromGui',1.0)
try:
AskForSavingOrNot = rospy.ServiceProxy(self.namespace+'SaveDataFromGui', SaveData)
# if button is pressed save data
if self._widget.ButtonRequestSave.isChecked():
# request controller to save data
reply = AskForSavingOrNot(True)
if reply.Saving == True:
# if controller receives message, we know it
print('Saving')
else:
# request controller to STOP saving data
reply = AskForSavingOrNot(False)
if reply.Saving == True:
# if controller receives message, we know it
print('Stopped Saving')
except rospy.ServiceException, e:
print "Service call failed: %s"%e
except:
print "Service not available ..."
pass
def nextSolution(self, query):
if query.client == self:
while not query.finished:
rospy.wait_for_service(self.prologNextSolutionService)
request = rospy.ServiceProxy(self.prologNextSolutionService,
PrologNextSolution)
try:
response = request(id = str(query.id))
except rospy.ServiceException, exception:
raise Exception(
"PrologNextSolution service request failed: %s" % exception)
if response.status == PrologNextSolutionResponse.OK:
yield json.loads(response.solution)
elif response.status == PrologNextSolutionResponse.NO_SOLUTION:
query.client = None
return
elif response.status == PrologNextSolutionResponse.WRONG_ID:
query.client = None
raise Exception("Prolog query id is invalid: "+
"Another client may have terminated your query")
elif response.status == PrologNextSolutionResponse.QUERY_FAILED:
query.client = None
raise Exception("Prolog query failed: %s" % response.solution)
else:
query.client = None
raise Exception("Prolog query status unknown: %d", response.status)
def send_mission(self, droid_id, mission):
service_addr = "/%s_%i/%s" % ( self.prefix, droid_id, Droid.SERVICE_MISSION )
rospy.wait_for_service( service_addr )
send_mission_proxy = rospy.ServiceProxy( service_addr, DroidMission )
send_mission_proxy( mission )
def main():
global client_speak
rospy.init_node("qbo_first_use")
rospy.loginfo("Launching Qbo First Use")
file_path = roslib.packages.get_pkg_dir("qbo_first_use") + "/FIRST_FILE"
file_exists = True
try:
with open(file_path) as f:
pass
except IOError as e:
file_exists = False
if file_exists:
rospy.loginfo("Qbo First Use program has terminated because this isn't Qbo first use")
return
else:
rospy.loginfo("Launching Qbo First use procedure")
client_speak = rospy.ServiceProxy("/qbo_talk/festival_say_no_wait", Text2Speach)
rospy.wait_for_service("/qbo_talk/festival_say_no_wait")
speak_this(
"This is my first boot... Please connect me to a local network by following the instructions on the manual"
)
open(file_path, "w").close()
def __init__(self,cam_marker_file,base_ee_file):
self.ready=False
#save file names
self.name_cam_marker_file=cam_marker_file
self.name_base_ee_file=base_ee_file
#count how many camera-marker poses in the file
self.camera_marker_file=open(self.name_cam_marker_file,'r')
self.num_of_lines_of_cam_marker_file=sum(1 for line in self.camera_marker_file)
self.num_of_cam_marker_poses=self.num_of_lines_of_cam_marker_file/3
#initiate marker poses w.r.t camera
self.camera_marker_samples=TransformArray()
self.camera_marker_samples.header.frame_id="/camera_rgb_optical_frame"
#count how many base-end-effector poses in the file
self.base_ee_file=open(self.name_base_ee_file,'r')
self.num_of_lines_of_base_ee_file=sum(1 for line in self.base_ee_file)
self.num_of_base_ee_poses=self.num_of_lines_of_base_ee_file
if self.num_of_cam_marker_poses != self.num_of_base_ee_poses:
print 'number of camera-marker poses not equal to number of base-ee poses!\n'
print 'please ctrl+c to exit\n'
self.read=False
#initiate ee poses w.r.t base
self.base_ee_samples=TransformArray()
self.base_ee_samples.header.frame_id="/base"
rospy.wait_for_service('compute_effector_camera_quick')
print 'waiting for service to be available '
self.calibrate=rospy.ServiceProxy('compute_effector_camera_quick',compute_effector_camera_quick)
print 'service server is ready'
def mommy(baby_name):
global x
global y
global theta
rospy.wait_for_service('spawn')
try:
new_turtle = rospy.ServiceProxy('spawn', Spawn)
if baby_name == "angel":
resp = new_turtle(5.5, 4.5, 0, baby_name)
else:
resp = new_turtle(5.5, 6.5, 0, baby_name)
except rospy.ServiceException, e:
print "%s" % e
rospy.wait_for_service(baby_name + '/set_pen')
try:
changepen = rospy.ServiceProxy(baby_name + '/set_pen', SetPen)
changepen(0, 0, 0, 1, 255)
except rospy.ServiceException, e:
print "%s" % e
if __name__ == '__main__':
baby_name = rospy.myargv(argv=sys.argv)[1]
mommy(baby_name)
try:
driver(baby_name)
except rospy.ROSInterruptException:
def __init__(self, id, initialPosition, tf):
self.id = id
prefix = "/cf" + str(id)
self.initialPosition = np.array(initialPosition)
self.tf = tf
rospy.wait_for_service(prefix + "/set_group_mask")
self.setGroupMaskService = rospy.ServiceProxy(
prefix + "/set_group_mask", SetGroupMask)
rospy.wait_for_service(prefix + "/takeoff")
self.takeoffService = rospy.ServiceProxy(prefix + "/takeoff", Takeoff)
rospy.wait_for_service(prefix + "/land")
self.landService = rospy.ServiceProxy(prefix + "/land", Land)
# rospy.wait_for_service(prefix + "/stop")
# self.stopService = rospy.ServiceProxy(prefix + "/stop", Stop)
rospy.wait_for_service(prefix + "/go_to")
self.goToService = rospy.ServiceProxy(prefix + "/go_to", GoTo)
rospy.wait_for_service(prefix + "/upload_trajectory")
self.uploadTrajectoryService = rospy.ServiceProxy(
prefix + "/upload_trajectory", UploadTrajectory)
rospy.wait_for_service(prefix + "/start_trajectory")
self.startTrajectoryService = rospy.ServiceProxy(
prefix + "/start_trajectory", StartTrajectory)
rospy.wait_for_service(prefix + "/update_params")
self.updateParamsService = rospy.ServiceProxy(
prefix + "/update_params", UpdateParams)
### added by The-SS begin
self.pubCmdFullState = rospy.Publisher(prefix + '/cmd_full_state',
FullState,
queue_size=1)
self.pubCmdStop = rospy.Publisher(prefix + '/cmd_stop',
Empty_msg,
queue_size=1)
self.worldFrame = '/world'
def __init__(self):
rospy.wait_for_service("/next_phase")
self.nextPhase = rospy.ServiceProxy("/next_phase", Empty)
def __init__(self):
# timing params
self.dt_sim = 0.01 # timestep of the simulation
self.t_activate = rospy.get_param('/t_activate')
self.t_final = rospy.get_param('/t_final')
# pop-up scenario params
self.s_ego_at_popup = rospy.get_param('/s_ego_at_popup')
self.s_obs_at_popup = rospy.get_param('/s_obs_at_popup')
self.d_obs_at_popup = rospy.get_param('/d_obs_at_popup')
# track params
self.track_name = rospy.get_param('/track_name')
self.s_begin_mu_segments = rospy.get_param('/s_begin_mu_segments')
self.mu_segment_values = rospy.get_param('/mu_segment_values')
self.N_mu_segments = len(self.s_begin_mu_segments)
self.mu_segment_idx = 0
# vehicle params
self.robot_name = rospy.get_param('/robot_name')
self.vehicle_width = rospy.get_param('/car/kinematics/l_width')
# algo params (from acado)
N = 40
dt_algo = 0.1
# set up pausing of gazebo
rospy.wait_for_service('gazebo/pause_physics')
pause_gazebo = rospy.ServiceProxy('gazebo/pause_physics', Empty)
rospy.wait_for_service('gazebo/unpause_physics')
unpause_gazebo = rospy.ServiceProxy('gazebo/unpause_physics', Empty)
self.s_pause_gazebo = rospy.get_param('/s_pause_gazebo')
n_pauses = len(self.s_pause_gazebo)
i_pauses = 0
# init node subs pubs
rospy.init_node('experiment_manager', anonymous=True)
#self.clockpub = rospy.Publisher('/clock', Clock, queue_size=10)
self.pathglobalsub = rospy.Subscriber("pathglobal", Path, self.pathglobal_callback)
self.statesub = rospy.Subscriber("state", State, self.state_callback)
self.carinfosub = rospy.Subscriber("/fssim/car_info", CarInfo, self.fssim_carinfo_callback)
self.trajstarsub = rospy.Subscriber("trajstar", Trajectory, self.trajstar_callback)
self.obspub = rospy.Publisher('/obs', Obstacles, queue_size=1)
self.obsvispub = rospy.Publisher('/obs_vis', Marker, queue_size=1)
self.tireparampub = rospy.Publisher('/tire_params', TireParams, queue_size=1)
self.ctrl_mode_pub = rospy.Publisher('/ctrl_mode', Int16, queue_size=1)
self.statetextmarkerpub = rospy.Publisher('/state_text_marker', Marker, queue_size=1)
# init logging vars
self.s_begin_log = rospy.get_param('/s_begin_log')
self.N_iters_to_save = 60
self.explog_iterationcounter = 0
self.explog_activated = False
self.stored_pathglobal = False
self.stored_trajstar = False
self.stored_trajcl = False
self.explog_saved = False
# init misc internal variables
self.pathglobal = Path()
self.received_pathglobal = False
self.state = State()
self.received_state = False
self.trajstar = Trajectory()
self.received_trajstar = False
self.fssim_carinfo = CarInfo()
while(not self.received_pathglobal):
print("waiting for pathglobal")
time.sleep(self.dt_sim*100)
# init experiment variables
self.scenario_id = rospy.get_param('/scenario_id')
self.traction_adaptive = rospy.get_param('/traction_adaptive')
self.tireparams = TireParams()
self.obs = Obstacles()
self.obs.s = [self.s_obs_at_popup]
self.obs.d = [self.d_obs_at_popup]
self.obs.R = [0.5]
wiggleroom = 1.0 # todo param
self.obs.Rmgn = [0.5*self.obs.R[0] + 0.5*self.vehicle_width + wiggleroom]
Xobs, Yobs = ptsFrenetToCartesian(np.array(self.obs.s), \
np.array(self.obs.d), \
np.array(self.pathglobal.X), \
np.array(self.pathglobal.Y), \
np.array(self.pathglobal.psi_c), \
np.array(self.pathglobal.s))
self.ctrl_mode = 0 # # 0: stop, 1: cruise_ctrl, 2: tamp
# Main loop
self.exptime = 0
while (not rospy.is_shutdown()) and self.exptime<self.t_final :
if (self.exptime >= self.t_activate):
rospy.loginfo_throttle(1, "Running experiment, ctrl mode = %i"%self.ctrl_mode)
# HANDLE TRACTION IN SIMULATION
s_ego = self.state.s % self.s_lap
for i in range(self.N_mu_segments-1):
if(self.s_begin_mu_segments[i] <= s_ego <= self.s_begin_mu_segments[i+1]):
self.mu_segment_idx = i
break
if(s_ego >= self.s_begin_mu_segments[-1]):
self.mu_segment_idx = self.N_mu_segments-1
mu = self.mu_segment_values[self.mu_segment_idx]
#print "s_ego = ", s_ego
#print "state.s = ", self.state.s
#print "self.N_mu_segments = ", self.N_mu_segments
#print "mu_segment_idx = ", self.mu_segment_idx
#print "mu in this section = ", self.mu_segment_values[self.mu_segment_idx]
# set tire params of sim vehicle
if (0.0 <= mu <0.3): # ice
B = 4.0
C = 2.0
D = mu
E = 1.0
elif (0.3 <= mu < 0.5): # snow
B = 5.0
C = 2.0
D = mu
E = 1.0
elif (0.5 <= mu < 0.9): # wet
B = 12.0
C = 2.3
D = mu
E = 1.0
elif (0.9 <= mu < 1.5): # dry
B = 10.0
C = 1.9
D = mu
E = 0.97
elif (1.5 <= mu < 2.5): # dry + racing tires (gotthard default)
B = 12.56;
C = 1.38;
D = mu;
E = 1.0
else:
rospy.loginfo_throttle(1, "Faulty mu value in exp manager")
self.tireparams.tire_coefficient = 1.0
self.tireparams.B = B
self.tireparams.C = C
self.tireparams.D = -D
self.tireparams.E = E
self.tireparams.header.stamp = rospy.Time.now()
self.tireparampub.publish(self.tireparams)
# POPUP SCENARIO
if (self.scenario_id == 1):
self.ctrl_mode = 1 # cruise control
if(self.state.vx > 5):
self.ctrl_mode = 2 # tamp
m_obs = self.getobstaclemarker(Xobs,Yobs,self.obs.R[0])
m_obs.color.a = 0.3 # transparent before detect
if (self.state.s >= self.s_ego_at_popup):
self.obspub.publish(self.obs)
m_obs.color.a = 1.0 # non-transparent after detect
self.obsvispub.publish(m_obs)
# REDUCED MU TURN
elif(self.scenario_id == 2):
self.ctrl_mode = 1 # pp cruise control from standstill
if(self.state.vx > 5): # todo
self.ctrl_mode = 2 # tamp cruise control when we get up to speed
# RACING
else:
self.ctrl_mode = 2 # tamp
# SEND STOP IF EXIT TRACK
dlb = np.interp(self.state.s,self.pathglobal.s,self.pathglobal.dlb)
dub = np.interp(self.state.s,self.pathglobal.s,self.pathglobal.dub)
if (self.state.d < dlb-1.0 or self.state.d > dub+1.0): # todo get from param
self.ctrl_mode = 0 # stop
self.ctrl_mode_pub.publish(self.ctrl_mode)
# publish text marker (state info)
if(self.traction_adaptive):
traction_adaptive_str = "on"
else:
traction_adaptive_str = "off"
state_text = "traction_adapt: " + traction_adaptive_str + "\n" \
"vx: " + "%.3f" % self.state.vx + "\n" \
"mu: " + "%.3f" % mu
self.statetextmarkerpub.publish(self.gettextmarker(state_text))
# handle pausing of gazebo
if (i_pauses < n_pauses):
if (self.state.s >= self.s_pause_gazebo[i_pauses]):
pause_gazebo()
raw_input("Press Enter to continue...")
unpause_gazebo()
i_pauses += 1
# save data for plot
if (self.state.s >= self.s_begin_log and not self.explog_activated):
print "STARTED EXPLOG"
t_start_explog = copy.deepcopy(self.exptime)
self.explog_activated = True
# store planned traj
self.trajstar_dict = {
"X": np.array(self.trajstar.X),
"Y": np.array(self.trajstar.Y),
"psi": np.array(self.trajstar.psi),
"s": np.array(self.trajstar.s),
"d": np.array(self.trajstar.d),
"deltapsi": np.array(self.trajstar.deltapsi),
"psidot": np.array(self.trajstar.psidot),
"vx": np.array(self.trajstar.vx),
"vy": np.array(self.trajstar.vy),
"ax": np.array(self.trajstar.ax),
"ay": np.array(self.trajstar.ay),
"Fyf": np.array(self.trajstar.Fyf),
"Fxf": np.array(self.trajstar.Fxf),
"Fyr": np.array(self.trajstar.Fyr),
"Fxr": np.array(self.trajstar.Fxr),
"Fzf": np.array(self.trajstar.Fzf),
"Fzr": np.array(self.trajstar.Fzr),
"kappac": np.array(self.trajstar.kappac),
"Cr": np.array(self.trajstar.Cr),
"t": np.arange(0,(N+1)*dt_algo,dt_algo),
}
self.stored_trajstar = True
# initialize vars for storing CL traj
self.X_cl = []
self.Y_cl = []
self.psi_cl = []
self.s_cl = []
self.d_cl = []
self.deltapsi_cl = []
self.psidot_cl = []
self.vx_cl = []
self.vy_cl = []
self.ax_cl = []
self.ay_cl = []
self.t_cl = []
self.Fyf_cl = []
self.Fyr_cl = []
self.Fx_cl = []
if (self.state.s >= self.s_begin_log and self.explog_activated and self.exptime >= t_start_explog + self.explog_iterationcounter*dt_algo):
# build CL traj
self.X_cl.append(self.state.X)
self.Y_cl.append(self.state.Y)
self.psi_cl.append(self.state.psi)
self.s_cl.append(self.state.s)
self.d_cl.append(self.state.d)
self.deltapsi_cl.append(self.state.deltapsi)
self.psidot_cl.append(self.state.psidot)
self.vx_cl.append(self.state.vx)
self.vy_cl.append(self.state.vy)
self.ax_cl.append(self.state.ax)
self.ay_cl.append(self.state.ay)
self.t_cl.append(self.exptime)
self.Fyf_cl.append(self.fssim_carinfo.Fy_f_l+self.fssim_carinfo.Fy_f_r)
self.Fyr_cl.append(self.fssim_carinfo.Fy_r)
self.Fx_cl.append(self.fssim_carinfo.Fx)
# store CL traj as dict
if (self.explog_iterationcounter == N): # store N+1 values, same as trajstar
self.trajcl_dict = {
"X": np.array(self.X_cl),
"Y": np.array(self.Y_cl),
"psi": np.array(self.psi_cl),
"s": np.array(self.s_cl),
"d": np.array(self.d_cl),
"deltapsi": np.array(self.deltapsi_cl),
"psidot": np.array(self.psidot_cl),
"vx": np.array(self.vx_cl),
"vy": np.array(self.vy_cl),
"ax": np.array(self.ax_cl),
"ay": np.array(self.ay_cl),
"t": np.array(self.t_cl),
"Fyf": np.array(self.Fyf_cl),
"Fyr": np.array(self.Fyr_cl),
"Fx": np.array(self.Fx_cl),
}
self.stored_trajcl = True
self.explog_iterationcounter +=1
# save explog
if (self.stored_pathglobal and self.stored_trajstar and self.stored_trajcl and not self.explog_saved):
explog = {
"pathglobal": self.pathglobal_dict,
"trajstar": self.trajstar_dict,
"trajcl": self.trajcl_dict,
}
filepath = "/home/larsvens/ros/tamp__ws/src/saarti/common/logs/"
filename = "explog"
if(self.scenario_id == 1):
filename = filename + "_popup"
if(np.min(self.mu_segment_values)>0.8):
filename = filename + "_dry"
else:
filename = filename + "_wet"
elif(self.scenario_id == 2):
filename = filename + "_reducedmuturn"
elif(self.scenario_id == 3):
filename = filename + "_racing"
if(self.traction_adaptive):
filename = filename + "_adaptive"
else:
filename = filename + "_nonadaptive"
filename = filename + ".npy"
np.save(filepath+filename,explog)
self.explog_saved = True
print("SAVED EXPLOG to " + filepath + filename)
else: # not reached activation time
rospy.loginfo_throttle(1, "Experiment starting in %i seconds"%(self.t_activate-self.exptime))
# handle exptime
self.exptime += self.dt_sim
msg = Clock()
t_rostime = rospy.Time(self.exptime)
msg.clock = t_rostime
#self.clockpub.publish(msg)
time.sleep(self.dt_sim)
print 'simulation finished'
# send shutdown signal
message = 'run finished, shutting down'
print message
rospy.signal_shutdown(message)
def __init__(self):
srv_nm = 'environment_server_right_arm/get_state_validity'
rospy.wait_for_service(srv_nm)
self.state_validator = rospy.ServiceProxy(srv_nm,
GetStateValidity,
persistent=True)
start_time = rospy.get_param('~start_time')
if start_time < 0.0:
rospy.logerr('Negative start time, setting it to 0.0')
start_time = 0.0
start_now = True
else:
start_now = False
else:
start_now = True
rospy.loginfo('Start time=%.2f s' % start_time)
interpolator = rospy.get_param('~interpolator', 'lipb')
try:
rospy.wait_for_service('init_waypoints_from_file', timeout=30)
except rospy.ROSException:
raise rospy.ROSException('Service not available! Closing node...')
try:
init_wp = rospy.ServiceProxy('init_waypoints_from_file',
InitWaypointsFromFile)
except rospy.ServiceException as e:
raise rospy.ROSException('Service call failed, error=%s', str(e))
success = init_wp(Time(rospy.Time.from_sec(start_time)), start_now,
String(filename), String(interpolator))
if success:
rospy.loginfo('Waypoints file successfully received, '
'filename=%s', filename)
with open (model_path + "cafe_table/model.sdf", "r") as table_file:
table_xml=table_file.read().replace('\n', '')
# Load Block URDF
block_xml = ''
with open (model_path + "block/model.urdf", "r") as block_file:
block_xml=block_file.read().replace('\n', '')
# Spawn Table SDF
rospy.wait_for_service('/gazebo/spawn_sdf_model')
try:
spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
resp_sdf = spawn_sdf("cafe_table", table_xml, "/",
table_pose, table_reference_frame)
except rospy.ServiceException, e:
rospy.logerr("Spawn SDF service call failed: {0}".format(e))
# Spawn Block URDF
rospy.wait_for_service('/gazebo/spawn_urdf_model')
try:
spawn_urdf = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
resp_urdf = spawn_urdf("block", block_xml, "/",
block_pose, block_reference_frame)
except rospy.ServiceException, e:
rospy.logerr("Spawn URDF service call failed: {0}".format(e))
def delete_gazebo_models():
# This will be called on ROS Exit, deleting Gazebo models
# Do not wait for the Gazebo Delete Model service, since
# Gazebo should already be running. If the service is not
# available since Gazebo has been killed, it is fine to error out
try:
delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
resp_delete = delete_model("cafe_table")
print "Got commands from param:\n%s" % commands_string
headers = headers_string.split()
cmd_all = [[float(x) for x in cur_line.split()]
for cur_line in commands_string.split("\n")]
cmd = [x for x in cmd_all if len(x) == len(headers)]
print "Commands"
print cmd
arm_controller = 'right_arm_trajectory_controller'
arm_query_topic = arm_controller + '/TrajectoryQuery'
arm_start_topic = arm_controller + '/TrajectoryStart'
print "Waiting Query Service: " + arm_query_topic
rospy.wait_for_service(arm_query_topic)
print "Found Query Service!"
query_srv = rospy.ServiceProxy(arm_query_topic, TrajectoryQuery)
query_resp = query_srv.call(TrajectoryQueryRequest(0))
head_publisher = rospy.Publisher('head_controller/command', JointStates)
print "Headers"
print headers
print "Queried Jointnames:"
print query_resp.jointnames
print "***** Remapping Arm Commands *****"
arm_mapping = [headers.index(x) for x in query_resp.jointnames]
print "Mapping:"
#!/usr/bin/env python
import rospy
from turtlesim.srv import SetPen
if __name__ == '__main__':
rospy.init_node('I_am_client')
rospy.wait_for_service('/turtle1/set_pen')
pen = rospy.ServiceProxy('/turtle1/set_pen', SetPen)
# put service call inside a try block to catch exceptions
# in case call fails
try:
pen(255,0,0,10,0)
# catch service exception
except rospy.ServiceException, error:
print "ops! call has failed with this error: ", error
#!/usr/bin/env python3
import rospy
from bitbots_msgs.msg import FootPressure
from bitbots_msgs.srv import FootScale, FootScaleRequest, FootScaleResponse
from std_srvs.srv import Empty, EmptyRequest, EmptyResponse
rospy.init_node("pressure_calibration")
rospy.loginfo(
"Welcome to the foot calibration suite. Press enter when there is no load on the weight cells"
)
input()
rospy.loginfo("waiting for service /set_foot_zero")
rospy.wait_for_service("/set_foot_zero")
zero = rospy.ServiceProxy("/set_foot_zero", Empty)
if zero():
rospy.loginfo("Successfully set the zero position for all sensors")
else:
rospy.loginfo("There was an error :(")
exit(1)
sensors = [
'left foot, left front',
'left foot, left back',
'left foot, right front',
'left foot, right back',
'right foot, left front',
'right foot, left back',
'right foot, right front',
'right foot, right back',
]
def compare_occmap(occmap1, occmap2):
global gettf_client
rospy.wait_for_service('GetMapTransform')
resp = gettf_client(occmap1, occmap2)
return resp.confidence
def initialize_commands():
rospy.init_node('mazesolvernode', anonymous=True)
rospy.wait_for_service('make_maze')
rospy.wait_for_service('print_maze')
rospy.wait_for_service('get_wall')
rospy.wait_for_service('get_odom')
rospy.wait_for_service('cs1567_move')
rospy.wait_for_service('cs1567_turn')
rospy.wait_for_service('cs1567_stop_all_motion')
# rospy.wait_for_service('constant_command')
global make_maze_service, print_maze_service, get_odom_service, get_wall_service
global constant_command_service
global move_service
global turn_service
global stop_service
global reset_odometry_service
make_maze_service = rospy.ServiceProxy('make_maze', MakeNewMaze)
print_maze_service = rospy.ServiceProxy('print_maze', Empty)
get_wall_service = rospy.ServiceProxy('get_wall', GetMazeWall)
get_odom_service = rospy.ServiceProxy('get_odom', GetOdometry)
# constant_command_service = rospy.ServiceProxy('constant_command', ConstantCommand)
move_service = rospy.ServiceProxy('cs1567_move', CS1567RobotMove)
turn_service = rospy.ServiceProxy('cs1567_turn', CS1567RobotTurn)
stop_service = rospy.ServiceProxy('cs1567_stop_all_motion', StopAll)
reset_odometry_service = rospy.ServiceProxy('cs1567_reset_odometry',
CSReset)
snd_pub = rospy.Publisher('/mobile_base/commands/sound', Sound)
led1_pub = rospy.Publisher('/mobile_base/commands/led1', Led)
led2_pub = rospy.Publisher('/mobile_base/commands/led2', Led)
make_maze_service(5, 5)
print_maze_service()
init_maze()
solve_maze(4, 4)
def resetWorld(self):
rospy.wait_for_service('/gazebo/reset_world')
try:
self.reset_world_proxy()
except rospy.ServiceException as e:
print("/gazebo/reset_world service call failed")
#!/usr/bin/env python
import rospy
import math
import tf2_ros
import geometry_msgs.msg
import turtlesim.srv
if __name__ == '__main__':
rospy.init_node('tf2_turtle_listener')
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
turtle_name = rospy.get_param('turtle', 'turtle2')
spawner(4, 2, 0, turtle_name)
turtle_vel = rospy.Publisher('%s/cmd_vel' % turtle_name,
geometry_msgs.msg.Twist,
queue_size=1)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
trans = tfBuffer.lookup_transform(turtle_name, 'turtle1',
rospy.Time())
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rate.sleep()
continue
#!/usr/bin/env python
import sys
import rospy
from aims_jackal_sim.srv import *
import numpy as np
if __name__ == "__main__":
rospy.wait_for_service('sim_reset')
srv = rospy.ServiceProxy('sim_reset', SimReset)
resp = srv(np.random.randint(low=0, high=1000))
if resp.result:
print('Sim reset successfully.')
else:
print('Failed to reset sim.')
def init_direction_service_client(self, service_name = "/crash_direction_service"):
rospy.loginfo('Waiting for Service Server')
rospy.wait_for_service(service_name) # wait for the service client /gazebo/delete_model to be running
rospy.loginfo('Service Server Found...')
self._direction_service = rospy.ServiceProxy(service_name, Trigger) # create the connection to the service
self._request_object = TriggerRequest()
def __init__(self, name):
""" Init the class """
# Save node name
self.name = name
# Get config
self.get_config()
# Create publisher
self.pub_thrusters = rospy.Publisher("/cola2_control/thrusters_data",
ThrustersData,
queue_size = 2)
self.pub_external_ra = rospy.Publisher("/cola2_safety/external_recovery_action",
RecoveryAction,
queue_size = 2)
# Init service clients
rospy.loginfo("%s: waiting for services", self.name)
try:
rospy.wait_for_service('/cola2_control/set_joystick_axes_to_velocity', 20)
self.set_joy_to_vel_srv = rospy.ServiceProxy(
'/cola2_control/set_joystick_axes_to_velocity', Empty)
except rospy.exceptions.ROSException:
self.captain_clients = False
rospy.logfatal("%s: set joystick axes to velocity service is not available!", self.name)
self.captain_clients = True
try:
rospy.wait_for_service('/cola2_control/disable_trajectory', 20)
self.abort_mission_srv = rospy.ServiceProxy(
'/cola2_control/disable_trajectory', Empty)
except rospy.exceptions.ROSException:
self.captain_clients = False
rospy.logfatal("%s: disable trajectory service not available!", self.name)
try:
rospy.wait_for_service('/cola2_control/disable_keep_position', 2)
self.abort_keep_pose_srv = rospy.ServiceProxy(
'/cola2_control/disable_keep_position', Empty)
except rospy.exceptions.ROSException:
self.captain_clients = False
rospy.logfatal("%s: disable keep position service not available!", self.name)
try:
rospy.wait_for_service('/cola2_control/disable_goto', 2)
self.abort_goto_srv = rospy.ServiceProxy(
'/cola2_control/disable_goto', Empty)
except rospy.exceptions.ROSException:
self.captain_clients = False
rospy.logfatal("%s: disable goto service not available!", self.name)
try:
rospy.wait_for_service('/cola2_control/submerge', 2)
self.surface_srv = rospy.ServiceProxy(
'/cola2_control/submerge', Submerge)
except rospy.exceptions.ROSException:
self.captain_clients = False
rospy.logfatal("%s: submerge service not available!", self.name)
if not self.captain_clients:
self.no_captain_clients_timer = rospy.Timer(rospy.Duration(0.4), self.no_captain_clients_message)
try:
rospy.wait_for_service('/cola2_control/disable_thrusters', 20)
self.abort_thrusters_srv = rospy.ServiceProxy(
'/cola2_control/disable_thrusters', Empty)
except rospy.exceptions.ROSException:
self.no_disable_thrusters_service_timer = rospy.Timer(rospy.Duration(0.4), self.no_disable_thrusters_message)
# Create service
self.recovery_srv = rospy.Service('/cola2_safety/recovery_action',
Recovery,
self.recovery_action_srv)
# Show message
rospy.loginfo("%s: initialized", self.name)
def resetSimulation(self):
rospy.wait_for_service('/gazebo/reset_simulation')
try:
self.reset_simulation_proxy()
except rospy.ServiceException as e:
print("/gazebo/reset_simulation service call failed")
def main(name,
poi_x='*',
poi_y='*',
poi_z='*',
ori_x='*',
ori_y='*',
ori_z='*',
ori_w='*',
lin_x='*',
lin_y='*',
lin_z='*',
ang_x='*',
ang_y='*',
ang_z='*',
timeout=None):
modelstate = ModelState()
pose = Pose()
twist = Twist()
pose, twist = gazebo_get_model_state.main(str(name), "world", timeout)
if (poi_x != '*'):
pose.position.x = float(poi_x)
if (poi_y != '*'):
pose.position.y = float(poi_y)
if (poi_z != '*'):
pose.position.z = float(poi_z)
if (ori_x != '*'):
pose.orientation.x = float(ori_x)
if (ori_y != '*'):
pose.orientation.y = float(ori_y)
if (ori_z != '*'):
pose.orientation.z = float(ori_z)
if (ori_w != '*'):
pose.orientation.w = float(ori_w)
if (lin_x != '*'):
twist.linear.x = float(lin_x)
if (lin_y != '*'):
twist.linear.y = float(lin_y)
if (lin_z != '*'):
twist.linear.z = float(lin_z)
if (ang_x != '*'):
twist.angular.x = float(ang_x)
if (ang_y != '*'):
twist.angular.y = float(ang_y)
if (ang_z != '*'):
twist.angular.z = float(ang_z)
modelstate.model_name = name
modelstate.pose = pose
modelstate.twist = twist
modelstate.reference_frame = "world"
try:
if (timeout == None):
rospy.wait_for_service('gazebo/set_model_state')
else:
rospy.wait_for_service('gazebo/set_model_state', int(timeout))
set_model_state_prox = rospy.ServiceProxy('gazebo/set_model_state',
SetModelState)
set_model_state_prox(modelstate)
print("gazebo/set_model_state %s!" % str(name))
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
def pr2_mover(object_list):
# TODO: Initialize variables
dict_list = []
pick_centroids = [] # to be list of tuples (x, y, z)
test_scene_num = Int32()
test_scene_num.data = 1
# TODO: Get/Read parameters
pick_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
dropbox = {}
for p in dropbox_param:
dropbox[p['name']] = p['position']
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
for pick in pick_list_param:
print(pick_list_param)
pick_name = pick['name']
pick_group = pick['group']
print("\nLooking for [{}] to be placed in [{}] box.".format(
pick_name, pick_group))
object_name = String()
object_name.data = pick_name
# TODO: Get the PointCloud for a given object and obtain it's centroid
# Index of the object to be picked in the `detected_objects` list
pick_cloud = None
for i, detected_object in enumerate(object_list):
if (detected_object.label == pick_name):
pick_cloud = detected_object.cloud
if (pick_cloud == None):
print("ERROR:::: {} not found in the detected object list".format(
pick_name))
continue
points_arr = ros_to_pcl(pick_cloud).to_array()
pick_centroid = np.mean(points_arr, axis=0)[:3]
pick_centroids.append(pick_centroid)
print("\nCentroid found : {}".format(pick_centroid))
pick_pose = Pose()
pick_pose.position.x = float(pick_centroid[0])
pick_pose.position.y = float(pick_centroid[1])
pick_pose.position.z = float(pick_centroid[2])
# TODO: Assign the arm to be used for pick_place
arm_name = String()
if pick['group'] == 'red':
arm_name.data = 'left'
elif pick['group'] == 'green':
arm_name.data = 'right'
else:
print "ERROR, group must be green or red!"
# TODO: Create 'place_pose' for the object
place_pose = Pose()
place_pose.position.x = dropbox[arm_name.data][0]
place_pose.position.y = dropbox[arm_name.data][1]
place_pose.position.z = dropbox[arm_name.data][2]
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
pick_pose, place_pose)
dict_list.append(yaml_dict)
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine',
PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(test_scene_num, object_name, arm_name,
pick_pose, place_pose)
print("Response: ", resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def pr2_mover(object_list):
# TODO: Initialize variables
object_name_list = []
object_group = []
dropbox_name = []
dropbox_group = []
dropbox_position = []
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_list_param = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
for i in range(len(object_list_param)):
object_name_list.append(object_list_param[i]['name'])
object_group.append(object_list_param[i]['group'])
for i in range(len(dropbox_list_param)):
dropbox_name.append(dropbox_list_param[i]['name'])
dropbox_group.append(dropbox_list_param[i]['group'])
dropbox_position.append(dropbox_list_param[i]['position'])
# TODO: Rotate PR2 in place to capture side tables for the collision map
labels = []
centroids = [] # to be list of tuples (x, y, z)
for object in object_list:
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
cntd = np.mean(points_arr, axis=0)[:3]
centroids.append(
[np.asscalar(cntd[0]),
np.asscalar(cntd[1]),
np.asscalar(cntd[2])])
# TODO: Loop through the pick list
test_no = 3
dict_list = []
for i in range(len(object_list_param)):
test_scene_num = Int32()
object_name = String()
arm_name = String()
pick_pose = Pose()
place_pose = Pose()
test_scene_num.data = test_no
object_name.data = object_name_list[i]
ind = search_in_list(object_name_list[i], labels)
if ind == -1:
continue
# TODO: Get the PointCloud for a given object and obtain it's centroid
pick_pose.position.x = centroids[ind][0]
pick_pose.position.y = centroids[ind][1]
pick_pose.position.z = centroids[ind][2]
# TODO: Create 'place_pose' for the object
assigned_group_name = object_group[i]
ind2 = search_in_list(assigned_group_name, dropbox_group)
place_pose.position.x = dropbox_position[ind2][0]
place_pose.position.y = dropbox_position[ind2][1]
place_pose.position.z = dropbox_position[ind2][2]
# TODO: Assign the arm to be used for pick_place
arm_name.data = dropbox_name[ind2]
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
pick_pose, place_pose)
dict_list.append(yaml_dict)
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine',
PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(test_scene_num, object_name, arm_name,
pick_pose, place_pose)
print("Response: ", resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def control():
# sp = np.load("xy_sin.npy")
state_sub = rospy.Subscriber("/mavros/state",
State,
state_cb,
queue_size=10)
rospy.wait_for_service('mavros/cmd/arming')
arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
rospy.wait_for_service('mavros/set_mode')
set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
acutator_control_pub = rospy.Publisher("/mavros/actuator_control",
ActuatorControl,
queue_size=10)
local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local",
PoseStamped,
queue_size=10)
mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose",
PoseStamped,
queue_size=10)
imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, imu_cb, queue_size=10)
local_pos_sub = rospy.Subscriber("/mavros/local_position/pose",
PoseStamped,
lp_cb,
queue_size=10)
local_vel_sub = rospy.Subscriber("/mavros/local_position/velocity",
TwistStamped,
lv_cb,
queue_size=10)
act_control_sub = rospy.Subscriber("/mavros/act_control/act_control_pub",
ActuatorControl,
act_cb,
queue_size=10)
rospy.init_node('control', anonymous=True)
rate = rospy.Rate(50.0)
# print("*"*80)
while not rospy.is_shutdown() and not current_state.connected:
rate.sleep()
rospy.loginfo(current_state.connected)
# print("#"*80)
pose = PoseStamped()
# mocap_pose = PoseStamped()
offb_set_mode = SetModeRequest()
offb_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
last_request = rospy.Time.now()
i = 0
act = ActuatorControl()
flag1 = False
flag2 = False
prev_imu_data = Imu()
prev_time = rospy.Time.now()
# prev_x = 0
# prev_y = 0
# prev_z = 0
# prev_vx = 0
# prev_vy = 0
# prev_vz = 0
prev_omega_x = 0
prev_omega_y = 0
prev_omega_z = 0
prev_vx = 0
prev_vy = 0
prev_vz = 0
delta_t = 0.02
count = 0
theta = 0.0
#rospy.loginfo("Outside")
r = np.random.rand(100000, 1) * 5
x_trgt = []
y_trgt = []
row = 0
theta = 0.0
x_step = 0.0
y_step = 0.0
for t in xrange(0, 100000):
# val = np.random.uniform(0,0.5)*5
# y_step = 0.0
for p in xrange(0, 50):
x_trgt.append(x_step)
y_trgt.append(r[t, 0] + y_step)
x_step += 0.01
y_step += 0.01
theta += 1.0 / 100
alpha = 0.01
temp = 0.0
y_new = []
for r_n in range(0, len(y_trgt)):
temp = temp + alpha * (y_trgt[r_n] - temp)
y_new.append(temp)
while not rospy.is_shutdown():
if current_state.mode != "OFFBOARD" and (
rospy.Time.now() - last_request > rospy.Duration(5.0)):
offb_set_mode_response = set_mode_client(offb_set_mode)
if offb_set_mode_response.mode_sent:
rospy.loginfo("Offboard enabled")
flag1 = True
last_request = rospy.Time.now()
else:
if current_state.armed == False:
arm_cmd_response = arming_client(arm_cmd)
if arm_cmd_response.success:
rospy.loginfo("Vehicle armed")
flag2 = True
last_request = rospy.Time.now()
# rospy.loginfo("Inside")
curr_time = rospy.Time.now()
if flag1 and flag2:
x_f.append(float(local_position.pose.position.x))
y_f.append(float(local_position.pose.position.y))
z_f.append(float(local_position.pose.position.z))
vx_f.append(float(local_velocity.twist.linear.x))
vy_f.append(float(local_velocity.twist.linear.y))
vz_f.append(float(local_velocity.twist.linear.z))
# print(local_velocity.twist.linear.x)
orientation = [
imu_data.orientation.w, imu_data.orientation.x,
imu_data.orientation.y, imu_data.orientation.z
]
(roll, pitch, yaw) = quaternion_to_euler_angle(
imu_data.orientation.w, imu_data.orientation.x,
imu_data.orientation.y, imu_data.orientation.z)
r_f.append(float(mt.radians(roll)))
p_f.append(float(mt.radians(pitch)))
yaw_f.append(float(mt.radians(yaw)))
sin_r_f.append(mt.sin(float(mt.radians(roll))))
sin_p_f.append(mt.sin(float(mt.radians(pitch))))
sin_y_f.append(mt.sin(float(mt.radians(yaw))))
cos_r_f.append(mt.cos(float(mt.radians(roll))))
cos_p_f.append(mt.cos(float(mt.radians(pitch))))
cos_y_f.append(mt.cos(float(mt.radians(yaw))))
rs_f.append(float(imu_data.angular_velocity.x))
ps_f.append(float(imu_data.angular_velocity.y))
ys_f.append(float(imu_data.angular_velocity.z))
ix.append(float(ixx))
iy.append(float(iyy))
iz.append(float(izz))
m.append(float(mass))
u0.append(act_controls.controls[0])
u1.append(act_controls.controls[1])
u2.append(act_controls.controls[2])
u3.append(act_controls.controls[3])
pose.pose.position.x = x_trgt[row]
pose.pose.position.y = y_new[row]
pose.pose.position.z = 3
row = row + 1
x_des.append(0)
y_des.append(0)
z_des.append(3)
sin_y_des.append(yaw)
cos_y_des.append(yaw)
w_mag.append(0)
w_x.append(0)
w_y.append(0)
w_z.append(0)
n_t = curr_time - prev_time
#delta_t = float(n_t.nsecs) * 1e-9
a_x.append(
(float(local_velocity.twist.linear.x) - prev_vx) / delta_t)
a_y.append(
(float(local_velocity.twist.linear.y) - prev_vy) / delta_t)
a_z.append(
(float(local_velocity.twist.linear.z) - prev_vz) / delta_t)
prev_vx = float(local_velocity.twist.linear.x)
prev_vy = float(local_velocity.twist.linear.y)
prev_vz = float(local_velocity.twist.linear.z)
aplha_x.append(
(float(imu_data.angular_velocity.x) - prev_omega_x) / delta_t)
aplha_y.append(
(float(imu_data.angular_velocity.y) - prev_omega_y) / delta_t)
aplha_z.append(
(float(imu_data.angular_velocity.z) - prev_omega_z) / delta_t)
ax_f.append(float(imu_data.linear_acceleration.x))
ay_f.append(float(imu_data.linear_acceleration.y))
az_f.append(float(imu_data.linear_acceleration.z))
prev_omega_x = float(imu_data.angular_velocity.x)
prev_omega_y = float(imu_data.angular_velocity.y)
prev_omega_z = float(imu_data.angular_velocity.z)
theta += 0.5
count += 1
local_pos_pub.publish(pose)
print('data_points =', data_points, " count = ", count, "row =",
row)
if (count >= data_points):
break
prev_time = curr_time
rate.sleep()
nn1_yaw_input_state = np.array([
vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
cos_p_f, cos_y_f, u3
],
ndmin=2).transpose()
nn1_yaw_grd_truth = np.array([a_x, a_y, a_z], ndmin=2).transpose()
nn2_yaw_input_state = np.array([
vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
cos_p_f, cos_y_f, u0, u1, u2
],
ndmin=2).transpose()
nn2_yaw_grd_truth = np.array([aplha_x, aplha_y, aplha_z],
ndmin=2).transpose()
print('nn1_test_traj_1_input_state :', nn1_yaw_input_state.shape)
print('nn1_test_traj_1_grd_truth :', nn1_yaw_grd_truth.shape)
print('nn2_test_traj_1_input_state :', nn2_yaw_input_state.shape)
print('nn2_test_traj_1_grd_truth :', nn2_yaw_grd_truth.shape)
np.save('nn1_test_traj_1_input_state.npy', nn1_yaw_input_state)
np.save('nn1_test_traj_1_grd_truth.npy', nn1_yaw_grd_truth)
np.save('nn2_test_traj_1_input_state.npy', nn2_yaw_input_state)
np.save('nn2_test_traj_1_grd_truth.npy', nn2_yaw_grd_truth)
s_test_traj_1 = np.array([
x_f, y_f, z_f, vx_f, vy_f, vz_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f,
cos_p_f, cos_y_f, rs_f, ps_f, ys_f, r_f, p_f, yaw_f
],
ndmin=2).transpose()
u_test_traj_1 = np.array([u0, u1, u2, u3], ndmin=2).transpose()
a_test_traj_1 = np.array([ax_f, ay_f, az_f], ndmin=2).transpose()
print('s_test_traj_1 :', s_test_traj_1.shape)
print('u_test_traj_1 :', u_test_traj_1.shape)
print('a_test_traj_1 :', a_test_traj_1.shape)
np.save('s_test_traj_1.npy', s_test_traj_1)
np.save('u_test_traj_1.npy', u_test_traj_1)
np.save('a_test_traj_1.npy', a_test_traj_1)
def main():
tags = {}
cup_tags = []
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
right_gripper = baxter_gripper.Gripper('right')
left_gripper = baxter_gripper.Gripper('left')
right_gripper.calibrate()
# left_gripper.calibrate()
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
rospy.wait_for_service('fixed_multi_service')
get_ar = rospy.ServiceProxy('fixed_multi_service', ARPose)
tag3 = get_ar(3)
tag4 = get_ar(4)
tag5 = get_ar(5)
cup_tags.append(tag3)
cup_tags.append(tag4)
cup_tags.append(tag5)
def get_closest_cup_tag(ar_tag, cup_tags):
def dist(x1, y1, z1, x2, y2, z2):
return (x1 - x2)**2 + (y1 - y2)**2 + (z1 - z2)**2
minDist = float('inf')
minTag = None
for T in cup_tags:
d = dist(ar_tag.pos_x, ar_tag.pos_y, ar_tag.pos_z, T.pos_x,
T.pos_y, T.pos_z)
if d < minDist:
minDist = d
minTag = T
return minTag
# starting position
os.system(
'rosrun cup_grabber joint_trajectory_file_playback.py -f starting_position'
)
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
lj = left.joint_names()
rj = right.joint_names()
def move(curr, trans):
a = 14 * np.pi / 180
cx, cy, cz = curr
tx, ty, tz = trans
nz = tz * np.cos(a) - ty * np.sin(a)
ny = tz * np.sin(a) + ty * np.cos(a)
return (cx + tx, cy + ny, cz + nz)
################### learning tag3
while tag3.tag_name == 'notfound':
tag3 = get_ar(3)
print 'tag3 not found'
cup_tags[0] = tag3
currxyz = (tag3.pos_x, tag3.pos_y, tag3.pos_z)
currori = (-.083, -.02, -.697, .712)
right_gripper.open()
currxyz = move(currxyz, (-.1, .11, -.1))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (0, 0, .17))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
right_gripper.close()
currxyz = move(currxyz, (0, .5, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (-.2, 0, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (.2, 0, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (0, -.5, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
right_gripper.open()
currxyz = move(currxyz, (0, 0, -.17))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
tag8 = get_ar(8)
tag9 = get_ar(9)
tag10 = get_ar(10)
if tag8.tag_name != 'notfound': tags[8] = tag8
if tag9.tag_name != 'notfound': tags[9] = tag9
if tag10.tag_name != 'notfound': tags[10] = tag9
################### learning tag4
while tag4.tag_name == 'notfound':
tag4 = get_ar(4)
print 'tag4 not found'
cup_tags[1] = tag4
currxyz = (tag4.pos_x, tag4.pos_y, tag4.pos_z)
currxyz = move(currxyz, (-.1, .11, -.1))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (0, 0, .17))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
right_gripper.close()
currxyz = move(currxyz, (0, .5, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (-.2, 0, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (.2, 0, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (0, -.5, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
right_gripper.open()
currxyz = move(currxyz, (0, 0, -.17))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
tag8 = get_ar(8)
tag9 = get_ar(9)
tag10 = get_ar(10)
if tag8.tag_name != 'notfound': tags[8] = tag8
if tag9.tag_name != 'notfound': tags[9] = tag9
if tag10.tag_name != 'notfound': tags[10] = tag9
################### learning tag5
while tag5.tag_name == 'notfound':
tag5 = get_ar(5)
print 'tag5 not found'
cup_tags[2] = tag5
currxyz = (tag5.pos_x, tag5.pos_y, tag5.pos_z)
currxyz = move(currxyz, (-.1, .1, -.1))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (0, 0, .17))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
right_gripper.close()
currxyz = move(currxyz, (0, .5, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (-.2, 0, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (.2, 0, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (0, -.5, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
right_gripper.open()
currxyz = move(currxyz, (0, 0, -.17))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
tag8 = get_ar(8)
tag9 = get_ar(9)
tag10 = get_ar(10)
if tag8.tag_name != 'notfound': tags[8] = tag8
if tag9.tag_name != 'notfound': tags[9] = tag9
if tag10.tag_name != 'notfound': tags[10] = tag9
############################################# done sensing the environment
################ pick up bottle with '8' contents
cup_tag_near_8 = get_closest_cup_tag(tag8, cup_tags)
curr = None
if cup_tag_near_8.tag_name == '3':
currxyz = (tag3.pos_x, tag3.pos_y, tag3.pos_z)
curr = 3
elif cup_tag_near_8.tag_name == '4':
currxyz = (tag4.pos_x, tag4.pos_y, tag4.pos_z)
curr = 4
elif cup_tag_near_8.tag_name == '5':
currxyz = (tag5.pos_x, tag5.pos_y, tag5.pos_z)
curr = 5
currxyz = move(currxyz, (-.1, .1, -.1))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (0, 0, .17))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
right_gripper.close()
currxyz = move(currxyz, (0, .5, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
os.system('cd ~/Desktop/baxter-waiter/src/cup_grabber/src')
os.system(
'rosrun cup_grabber joint_trajectory_file_playback.py -f left_starting_position'
)
os.system('cd ~/Desktop/baxter-waiter/src/cup_grabber/src')
os.system(
'rosrun cup_grabber joint_trajectory_file_playback.py -f right_pouring'
)
currxyz = move(currxyz, (0, -.5, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
right_gripper.open()
currxyz = move(currxyz, (0, 0, -.17))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
################ pick up bottle with '9' contents
cup_tag_near_9 = get_closest_cup_tag(tag9, cup_tags)
if cup_tag_near_9.tag_name == '3':
currxyz = (tag3.pos_x, tag3.pos_y, tag3.pos_z)
curr = 3
elif cup_tag_near_9.tag_name == '4':
currxyz = (tag4.pos_x, tag4.pos_y, tag4.pos_z)
curr = 4
elif cup_tag_near_9.tag_name == '5':
currxyz = (tag5.pos_x, tag5.pos_y, tag5.pos_z)
curr = 5
currxyz = move(currxyz, (-.1, .1, -.1))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (0, 0, .17))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
right_gripper.close()
currxyz = move(currxyz, (0, .5, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
os.system('cd ~/Desktop/baxter-waiter/src/cup_grabber/src')
os.system(
'rosrun cup_grabber joint_trajectory_file_playback.py -f right_pouring'
)
currxyz = move(currxyz, (0, -.5, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
right_gripper.open()
currxyz = move(currxyz, (0, 0, -.17))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
################ pick up bottle with '10' contents
cup_tag_near_10 = get_closest_cup_tag(tag10, cup_tags)
if cup_tag_near_10.tag_name == '3':
currxyz = (tag3.pos_x, tag3.pos_y, tag3.pos_z)
curr = 3
elif cup_tag_near_10.tag_name == '4':
currxyz = (tag4.pos_x, tag4.pos_y, tag4.pos_z)
curr = 4
elif cup_tag_near_10.tag_name == '5':
currxyz = (tag5.pos_x, tag5.pos_y, tag5.pos_z)
curr = 5
currxyz = move(currxyz, (-.1, .1, -.1))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
currxyz = move(currxyz, (0, 0, .17))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
right_gripper.close()
currxyz = move(currxyz, (0, .5, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
os.system('cd ~/Desktop/baxter-waiter/src/cup_grabber/src')
os.system(
'rosrun cup_grabber joint_trajectory_file_playback.py -f right_pouring'
)
currxyz = move(currxyz, (0, -.5, 0))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
right_gripper.open()
currxyz = move(currxyz, (0, 0, -.17))
requestIK('right_gripper', 'right_arm', currxyz, currori, compute_ik)
os.system('cd ~/Desktop/baxter-waiter/src/cup_grabber/src')
os.system('rosrun cup_grabber joint_trajectory_file_playback.py -f serve')
os.system('cd ~/Desktop/baxter-waiter/src/cup_grabber/src')
os.system('rosrun cup_grabber joint_trajectory_file_playback.py -f dab')
goal_pose = Odometry()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = 'world'
goal_pose.pose.pose.position = Point(pose[0][0]+x_offset, pose[0][1]+y_offset, 0.)
return goal_pose
def get_result(result_aux):
global result
result.data = result_aux.data
if __name__ == '__main__':
pub = rospy.Publisher('move_usv/goal', Odometry, queue_size=10)
rospy.init_node('patrol')
rate = rospy.Rate(1) # 10h
rospy.Subscriber("move_usv/result", Float64, get_result)
rospy.wait_for_service('/gazebo/unpause_physics')
rospy.wait_for_service('/gazebo/pause_physics')
rospy.wait_for_service('/gazebo/reset_simulation')
unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
resetSimulation = rospy.ServiceProxy('/gazebo/reset_simulation', Empty)
#unpause()
simulationNumber = 1
while not rospy.is_shutdown():
try:
rospy.logerr("Simulation number %d", simulationNumber)
for pose in waypoints:
goal = goal_pose(pose)
pub.publish(goal)
def keyPressEvent(self, event):
key = event.key()
# If we have constructed the drone controller and the key is not generated from an auto-repeating key
if controller is not None and not event.isAutoRepeat():
# Handle the important cases first!
if key == KeyMapping.ThrottleCut:
controller.SendEmergency()
elif key == KeyMapping.Emergency:
controller.SendLand() # modify to prevent damage to quad
elif key == KeyMapping.Takeoff:
controller.SendTakeoff()
elif key == KeyMapping.Land:
controller.SendLand()
elif key == KeyMapping.Linear:
controller.SendLinear()
elif key == KeyMapping.Circle:
controller.SendCircle()
elif key == KeyMapping.Spiral:
controller.SendSpiral()
elif key == KeyMapping.Snake:
controller.SendSnake()
elif key == KeyMapping.Ready:
controller.SendReady()
elif key == KeyMapping.ToggleProcessing:
controller.StartStop()
elif key == KeyMapping.CamChange:
rospy.wait_for_service('ardrone/togglecam')
try:
switchcam = rospy.ServiceProxy('ardrone/togglecam',
ToggleCam)
switchcam()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
elif key == KeyMapping.ProcessImg:
if self.processImagesBool == False:
self.processImagesBool = True
print "Image processing is turned on"
super(KeyboardController, self).EnableImageProcessing()
else:
self.processImagesBool = False
print "Image processing is turned off"
super(KeyboardController, self).DisableImageProcessing()
else:
# Now we handle moving, notice that this section is the opposite (+=) of the keyrelease section
if key == KeyMapping.YawLeft:
self.yaw_velocity += 1
elif key == KeyMapping.YawRight:
self.yaw_velocity += -1
elif key == KeyMapping.PitchForward:
self.pitch += 1
elif key == KeyMapping.PitchBackward:
self.pitch += -1
elif key == KeyMapping.RollLeft:
self.roll += 1
elif key == KeyMapping.RollRight:
self.roll += -1
elif key == KeyMapping.IncreaseAltitude:
self.z_velocity += 1
elif key == KeyMapping.DecreaseAltitude:
self.z_velocity += -1
# finally we set the command to be sent. The controller handles sending this at regular intervals
controller.SetCommand(self.roll, self.pitch, self.yaw_velocity,
self.z_velocity)
def onMessage(self, payload, isBinary):
# Debug
if isBinary:
print("Binary message received: {0} bytes".format(len(payload)))
else:
print("Text message received: {0}".format(payload.decode('utf8')))
# Do stuff
# pub = rospy.Publisher('blockly', String, queue_size=10)
# time.sleep(1)
# pub.publish("blockly says: "+payload.decode('utf8'))
# Simple text protocol for communication
# first line is the name of the method
# next lines are body of message
message_text = payload.decode('utf8')
message_data = message_text.split('\n', 1)
if len(message_data) > 0:
method_name = message_data[0]
if len(message_data) > 1:
method_body = message_data[1]
if method_name.startswith('play'):
CodeStatus.set_current_status(CodeStatus.RUNNING)
BlocklyServerProtocol.build_ros_node(method_body)
rospy.loginfo('The file generated contains...')
os.system('cat test.py')
if method_name == 'play2':
CodeExecution.run_process(['python', 'test.py'])
elif method_name == 'play3':
CodeExecution.run_process(['python3', 'test.py'])
else:
rospy.logerr('Called unknown method %s', method_name)
else:
if 'pause' == method_name:
CodeStatus.set_current_status(CodeStatus.PAUSED)
elif 'resume' == method_name:
CodeStatus.set_current_status(CodeStatus.RUNNING)
elif 'end' == method_name:
#End test.py execution
global pid
print("@@@@@@@@@@@@@@@@@@")
try:
print("kill pid="+str(pid))
os.kill(pid, signal.SIGKILL)
ros_nodes = rosnode.get_node_names()
except NameError:
print("execution script not running.")
pass
if '/imu_talker' in ros_nodes: #brain
##set default values
pub = rospy.Publisher('/statusleds', String, queue_size=10, latch=True)
msg = 'blue_off'
pub.publish(msg)
msg = 'orange_off'
pub.publish(msg)
if '/crab_leg_kinematics' in ros_nodes: #spider
print("spider running")
pub = rospy.Publisher('/joy', Joy, queue_size=10, latch=True)
msg = Joy()
msg.header.stamp = rospy.Time.now()
rate = rospy.Rate(10)
valueAxe = 0.0
valueButton = 0
for i in range (0, 20):
msg.axes.append(valueAxe)
for e in range (0, 17):
msg.buttons.append(valueButton)
pub.publish(msg)
print("DEFAULT MESSAGES SENT")
if '/mavros' in ros_nodes: #rover
print("rover")
pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)
msg = OverrideRCIn()
msg.channels[0] = 1500
msg.channels[1] = 0
msg.channels[2] = 1500
msg.channels[3] = 0
msg.channels[4] = 0
msg.channels[5] = 0
msg.channels[6] = 0
msg.channels[7] = 0
pub.publish(msg)
print("@@@@@@@@@@@@@@@@@@")
elif method_name.startswith('control'):
robot = method_name.split('control_')[1]
if robot.startswith('spider'):
direction = robot.split('spider_')[1]
pub = rospy.Publisher('/joy', Joy, queue_size=10)
msg = Joy()
msg.header.stamp = rospy.Time.now()
rate = rospy.Rate(10)
valueAxe = 0.0
valueButton = 0
for i in range (0, 20):
msg.axes.append(valueAxe)
for e in range (0, 17):
msg.buttons.append(valueButton)
if direction == 'up':#forward
msg.axes[1] = 1
elif direction == 'down':#backwards
msg.axes[1] = -1
elif direction == 'left':#turn left
#msg.axes[0] = 1
msg.axes[2] = 1
elif direction == 'right':#turn rigth
#msg.axes[0] = -1
msg.axes[2] = -1
pub.publish(msg)
rate.sleep()
elif robot.startswith('rover'):
direction = robot.split('rover_')[1]
rospy.wait_for_service('/mavros/set_mode')
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
resp1 = change_mode(custom_mode='manual')
print (resp1)
if 'True' in str(resp1):
pub = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=10)
r = rospy.Rate(10) #2hz
msg = OverrideRCIn()
throttle_channel=2
steer_channel=0
speed_slow = 1558
#speed_turbo = 2000 #dangerous
speed = speed_slow
if direction == 'up':#forward
msg.channels[throttle_channel]=speed
msg.channels[steer_channel]=1385 #straight
elif direction == 'down':#backwards
msg.channels[throttle_channel]=1450 #slow
msg.channels[steer_channel]=1385 #straight
elif direction == 'left':#turn left
msg.channels[throttle_channel]=speed
msg.channels[steer_channel]=1285
elif direction == 'right':#turn rigth
msg.channels[throttle_channel]=speed
msg.channels[steer_channel]=1485
start = time.time()
flag=True
while not rospy.is_shutdown() and flag:
sample_time=time.time()
if ((sample_time - start) > 0.5):
flag=False
pub.publish(msg)
r.sleep()
else:
rospy.logerr('Called unknown method %s', method_name)
def main():
global objInfos
rospy.init_node('sequencer')
move_to_homepose = rospy.ServiceProxy('iktest_controller/move_to_homepose',
Trigger)
move_to_cup_offset = rospy.ServiceProxy(
'iktest_controller/move_to_cup_offset', OffsetMove)
pick_up_dice_above = rospy.ServiceProxy(
'iktest_controller/pick_up_dice_above', OffsetMove)
pick_up_dice = rospy.ServiceProxy('iktest_controller/pick_up_dice',
OffsetMove)
pour_dice = rospy.ServiceProxy('iktest_controller/pour_dice', Trigger)
#rospy.wait_for_service('iktest_controller/move_to_cup_offset', 3.0)
rospy.wait_for_service('iktest_controller/pick_up_dice', 3.0)
close_grip = rospy.ServiceProxy('gripper_controller_test/close_grip',
Trigger)
open_grip = rospy.ServiceProxy('gripper_controller_test/open_grip',
Trigger)
# Stored offset pose information
'''
dice_pose = Pose(
position = Point(
x = 0.776647640113,
y =-0.0615496888226,
z = -0.210376983209),
orientation = Quaternion(
x = 0.981404960951,
y = -0.19031770757,
z = 0.016510737149,
w = -0.0187314806041
)
)
'''
# Main process loop
while (True):
# Step 1: pour dices out of the cup
open_grip()
move_to_homepose()
rospy.sleep(1)
pour_dice()
# Step 2: Start loop of picking up dices
cnt_round = 0
FAILURE_TIME = 0
while FAILURE_TIME < 10:
cnt_round += 1
if cnt_round > 15:
break
print "---------- THE %dth ROUND ------------" % cnt_round
move_to_homepose()
dice_pose = call_cv_service_detect_all_dices()
if dice_pose is None:
print "No dice"
FAILURE_TIME += 1
continue
else:
print "Detect the dice ... "
def set_dice_orientation(dice_pose):
dice_pose.position.z = -0.207973876142
dice_pose.orientation.x = 0.5
dice_pose.orientation.y = 0.0
dice_pose.orientation.z = 0.0
dice_pose.orientation.w = -0.0
print "The dice pos detected by camera is:\n", dice_pose
return dice_pose
def set_dice_above(dice_pose):
dice_pose.position.z += 0.1
print "The above position for the dice is:\n", dice_pose
return dice_pose
dice_pose = set_dice_orientation(dice_pose) # set quaternion
# --------------------------------------
#OFFSET_Z=0.06
#dice_pose.position.z = dice_pose.position.z +OFFSET_Z
dice_pose = set_dice_above(dice_pose)
pick_up_dice_above(dice_pose)
# Refine
dice_pose = call_cv_service_detect_all_dices()
# dice_pose = call_cv_service_detect_one_dice()
if dice_pose is None:
print "During refining the dice pos, no dice is found."
FAILURE_TIME += 1
continue
else:
print "Refine dice pos successful"
'''
# -------------- Publish dice info to the topic
pretend.state="Dice Read"
pretend.turn=1
pretend.roll=cnt_round
pretend.dice1=1
pretend.dice2=2
pretend.dice3=3
pretend.dice4=4
pretend.dice5=5
pretend.dice1color='b'
pretend.dice2color='b'
pretend.dice3color='r'
pretend.dice4color='bl'
pretend.dice5color='y'
nobj=len(objInfos)
nthobj=0
if nobj>nthobj:
pretend.dice1=objInfos[nthobj].value
pretend.dice1color=objInfos[nthobj].color
nthobj=1
if nobj>nthobj:
pretend.dice1=objInfos[nthobj].value
pretend.dice1color=objInfos[nthobj].color
nthobj=2
if nobj>nthobj:
pretend.dice1=objInfos[nthobj].value
pretend.dice1color=objInfos[nthobj].color
nthobj=3
if nobj>nthobj:
pretend.dice1=objInfos[nthobj].value
pretend.dice1color=objInfos[nthobj].color
nthobj=4
if nobj>nthobj:
pretend.dice1=objInfos[nthobj].value
pretend.dice1color=objInfos[nthobj].color
# --------------
'''
# Gripper goes to the pos which is OFFSET_Z above the dice
dice_pose = set_dice_orientation(dice_pose)
#dice_pose.position.z = dice_pose.position.z +OFFSET_Z
dice_pose = set_dice_above(dice_pose)
pick_up_dice_above(dice_pose)
rospy.sleep(1)
# Gripper goes to grab the dice
dice_pose.position.z = dice_pose.position.z - 0.1
print "Height of dice: ", dice_pose.position.z
pick_up_dice(dice_pose) #//need Offset
rospy.sleep(1)
rospy.loginfo("Sequence complete.")
break
return
import rospy
import dynamic_reconfigure.client
def callback(config):
rospy.loginfo(
"Config set to {int_param}, {double_param}, {str_param}, {bool_param}, {size}"
.format(**config))
if __name__ == "__main__":
rospy.init_node("dynamic_client")
rospy.wait_for_service("dynamic_tutorials")
client = dynamic_reconfigure.client.Client("dynamic_tutorials",
timeout=30,
config_callback=callback)
r = rospy.Rate(0.1)
x = 0
b = False
while not rospy.is_shutdown():
x = x + 1
if x > 10:
x = 0
b = not b
client.update_configuration({
"int_param": x,
"double_param": (1 / (x + 1)),
import object_manipulation_msgs.msg
import geometry_msgs.msg
import sensor_msgs.msg
from numpy import *
import openravepy
import pickle
if __name__ == '__main__':
env = openravepy.Environment()
body = env.ReadKinBodyXMLFile('data/ketchup.kinbody.xml')
env.AddKinBody(body, True)
trimesh = env.Triangulate(body)
env.Remove(body)
env.Destroy()
rospy.init_node('graspplanning_test')
rospy.wait_for_service('GraspPlanning')
GraspPlanningFn = rospy.ServiceProxy(
'GraspPlanning', object_manipulation_msgs.srv.GraspPlanning)
req = object_manipulation_msgs.srv.GraspPlanningRequest()
req.arm_name = 'arm'
#req.target.type = object_manipulation_msgs.msg.GraspableObject.POINT_CLUSTER
req.target.cluster.header.frame_id = 'Base'
offset = [0.3, -0.05, 0.3]
req.target.cluster.points = [
geometry_msgs.msg.Point32(p[0] + offset[0], p[1] + offset[1],
p[2] + offset[2]) for p in trimesh.vertices
]
req.target.cluster.channels.append(
sensor_msgs.msg.ChannelFloat32(
name='indices', values=trimesh.indices.flatten().tolist()))
req.collision_object_name = ''
def __init__(self):
rospy.wait_for_service('testit/bringup')
rospy.wait_for_service('testit/teardown')
rospy.wait_for_service('testit/status')
rospy.wait_for_service('testit/test')
rospy.wait_for_service('testit/results')
rospy.wait_for_service('testit/bag/collect')
rospy.wait_for_service('testit/clean')
rospy.wait_for_service('testit/uppaal/annotate/coverage')
rospy.wait_for_service('testit/uppaal/extract/failure')
rospy.wait_for_service('testit/shutdown')
rospy.wait_for_service('testit/credits')
rospy.wait_for_service('testit/optimize')
rospy.wait_for_service('testit/learn')
self.bringup_service = rospy.ServiceProxy('testit/bringup',
testit.srv.Command)
self.teardown_service = rospy.ServiceProxy('testit/teardown',
testit.srv.Command)
self.status_service = rospy.ServiceProxy('testit/status',
testit.srv.Command)
self.test_service = rospy.ServiceProxy('testit/test',
testit.srv.Command)
self.learn_service = rospy.ServiceProxy('testit/learn',
testit.srv.Command)
self.results_service = rospy.ServiceProxy('testit/results',
testit.srv.Command)
self.bag_collect_service = rospy.ServiceProxy('testit/bag/collect',
testit.srv.Command)
self.clean_service = rospy.ServiceProxy('testit/clean',
testit.srv.Command)
self.uppaal_annotate_coverage_service = rospy.ServiceProxy(
'testit/uppaal/annotate/coverage', testit.srv.Command)
self.uppaal_extract_failure_service = rospy.ServiceProxy(
'testit/uppaal/extract/failure', testit.srv.Command)
self.shutdown_service = rospy.ServiceProxy('testit/shutdown',
testit.srv.Command)
self.credits_service = rospy.ServiceProxy('testit/credits',
testit.srv.Command)
self.optimize_service = rospy.ServiceProxy('testit/optimize',
testit.srv.Command)
self.log_service = rospy.ServiceProxy('testit/log', testit.srv.Command)