def main():
global X
global Y
global Color
#while loop to fetch all 5 waypoints from the server and do its thing
first = True
count = 0
#get the first waypoint
coorX = 0
coorY = 0
dict = rosservice.call_service("/Final_ints", [True, False, 0, 0, 0])[1]
if (dict.success):
coorX = dict.waypointx
coorY = dict.waypointy
else:
print("Something went wrong")
pass
while (count < 5):
#########
#Farah's script to move to waypoint w coorX and coorY
#########
print("Calling move script with coordinates (%.2f,%.2f)" %
(coorX, coorY))
#once move script is done, we will detect image
time.sleep(2)
#########
#Chris' script to detect colour
#########
#should return the colour of the star and its coordinates in the robot frame
objCoorX = X[count]
objCoorY = Y[count]
colour = Color[count]
#########
#transfrom lookingScript coordinates to world frame
#
#camera frame: camera_realsense_gazebo
#world frame: odom
#
#objCoorX = transfromed X coordinate
#objCoorY = transformed Y coordinate
#########
#communicate with the service with arguments: False True colour coorX coorY
dict = rosservice.call_service(
"/Final_ints", [False, True, colour, objCoorX, objCoorY])[1]
if (dict.success):
print("object detected successfully")
coorX = dict.waypointx
coorY = dict.waypointy
count += 1
else:
print("wrong colour, keep looking")
def myhook():
print("shutdown begin")
rospy.wait_for_service('/yumi/rws/sm_addin/stop_egm')
rosservice.call_service('/yumi/rws/sm_addin/stop_egm', {})
rospy.sleep(0.7)
rospy.wait_for_service('/yumi/rws/stop_rapid')
rosservice.call_service('/yumi/rws/stop_rapid', {})
rospy.sleep(0.7)
print("shutdown commplete")
def perform_service_call(service_name):
"""
POST /api/<version>/service/<service_name>
POST a message to a ROS service by name.
"""
service_name = "/" + service_name
args = request.json
if not args:
args = request.values.to_dict()
args = {
k: str(v) if isinstance(v, unicode) else v
for k, v in args.items()
}
try:
rospy.wait_for_service(service_name, 1)
except rospy.ROSException as e:
raise socket.error()
try:
res = rosservice.call_service(service_name, [args])[1]
except rosservice.ROSServiceException as e:
return error(e)
status_code = 200 if getattr(res, "success", True) else 400
data = {k: getattr(res, k) for k in res.__slots__}
return jsonify(data), status_code
def _rosapp_stop(args, robot=None):
matches = []
if robot is None:
matches.extend(rosservice.rosservice_find('app_manager/StopApp'))
else:
matches.append('/' + str(robot) + '/stop_app')
for match in matches:
(req, resp) = rosservice.call_service(match, args, StopApp)
print resp
def _rosapp_list(running=True, available=True, verbose=False, robot=None):
try:
matches = rosservice.rosservice_find('app_manager/ListApps')
except:
pass
for match in matches:
(req, resp) = rosservice.call_service(match, None, ListApps)
if running:
for app in resp.running_apps:
print str(app.name)
if available:
for app in resp.available_apps:
print str(app.name)
def safety_stop(self, ptAtual, wristPt):
# High limit in meters of the end effector relative to the base_link
high_limit = 0.03
# Does not allow wrist_1_link to move above 20 cm relative to base_link
high_limit_wrist_pt = 0.2
if ptAtual[-1] < high_limit or wristPt[-1] < high_limit_wrist_pt:
# Be careful. Only the limit of the end effector is being watched but the other
# joint can also exceed this limit and need to be carefully watched by the operator
rospy.loginfo("High limit of " + str(high_limit) + " exceeded!")
self.stop_robot()
rosservice.call_service('/controller_manager/switch_controller',
[['pos_based_pos_traj_controller'],
['joint_group_vel_controller'], 1])
raw_input("\n==== Press enter to home the robot again!")
joint_values = ur5_vel.get_ik([-0.4, -0.1, 0.5 + 0.15])
ur5_vel.home_pos(joint_values)
rosservice.call_service('/controller_manager/switch_controller',
[['joint_group_vel_controller'],
['pos_based_pos_traj_controller'], 1])
raw_input("\n==== Press enter to restart APF function!")
def perform_service_call(service_name):
"""
POST /api/<version>/service/<service_name>
POST a message to a ROS service by name.
"""
service_name = "/" + service_name
args = request.json
if not args:
args = request.values.to_dict()
args = {
k: str(v) if isinstance(v, unicode) else v for k,v in args.items()
}
try:
rospy.wait_for_service(service_name, 1)
except rospy.ROSException as e:
raise socket.error()
try:
res = rosservice.call_service(service_name, [args])[1]
except rosservice.ROSServiceException as e:
return error(e)
status_code = 200 if getattr(res, "success", True) else 400
data = {k: getattr(res, k) for k in res.__slots__}
return jsonify(data), status_code
def bag_service_client_stop():
rospy.wait_for_service('/logger/stop')
try:
rosservice.call_service('/logger/stop', None)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def bag_service_client_stop():
rospy.wait_for_service('logger/stop')
try:
rosservice.call_service('logger/stop', None)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
#! /usr/bin/python
import roslib; roslib.load_manifest('kelsey_sandbox')
import rospy
import roslib.message
import rosbag
import sys
import rosservice
from datetime import datetime
from hrl_lib.util import save_pickle
import yaml
#from hrl_lib import *
#from tabletop_object_detector.srv import TabletopDetection
if __name__ == "__main__":
service_args = []
for arg in sys.argv[2:]:
if arg == '':
arg = "''"
service_args.append(yaml.load(arg))
req, resp = rosservice.call_service(sys.argv[1], service_args)
a = datetime.now().isoformat('-')
name = '-'.join(a.split(':')).split('.')[0]
save_pickle(resp, sys.argv[1].split('/')[-1] + name + ".pickle")
#! /usr/bin/python
import roslib
roslib.load_manifest('kelsey_sandbox')
import rospy
import roslib.message
import rosbag
import sys
import rosservice
from datetime import datetime
from hrl_lib.util import save_pickle
import yaml
#from hrl_lib import *
#from tabletop_object_detector.srv import TabletopDetection
if __name__ == "__main__":
service_args = []
for arg in sys.argv[2:]:
if arg == '':
arg = "''"
service_args.append(yaml.load(arg))
req, resp = rosservice.call_service(sys.argv[1], service_args)
a = datetime.now().isoformat('-')
name = '-'.join(a.split(':')).split('.')[0]
save_pickle(resp, sys.argv[1].split('/')[-1] + name + ".pickle")
print("shutdown begin")
rospy.wait_for_service('/yumi/rws/sm_addin/stop_egm')
rosservice.call_service('/yumi/rws/sm_addin/stop_egm', {})
rospy.sleep(0.7)
rospy.wait_for_service('/yumi/rws/stop_rapid')
rosservice.call_service('/yumi/rws/stop_rapid', {})
rospy.sleep(0.7)
print("shutdown commplete")
if __name__ == "__main__":
rospy.wait_for_service('/yumi/rws/stop_rapid')
rosservice.call_service('/yumi/rws/stop_rapid', {})
rospy.sleep(1.5)
rospy.wait_for_service('/yumi/rws/pp_to_main')
rosservice.call_service('/yumi/rws/pp_to_main', {})
rospy.sleep(1.5)
rospy.wait_for_service('/yumi/rws/start_rapid')
rosservice.call_service('/yumi/rws/start_rapid', {})
rospy.sleep(1.5)
setup_uc = dict(use_filtering=True, comm_timeout=1.0)
xyz = dict(x=0.0, y=0.0, z=0.0)
quat = dict(q1=1.0, q2=0.0, q3=0.0, q4=0.0)
tframe = dict(trans=xyz, rot=quat)
#tload = dict(mass=0.001, cog=dict(x=0.0, y=0.0, z=0.001), aom=quat, ix=0.0, iy=0.0, iz=0.0)
#mass [kg], cog [mm], inertia [kgm^2]
tload = dict(
def turn_position_controller_on():
rosservice.call_service('/controller_manager/switch_controller', [['pos_based_pos_traj_controller'], ['joint_group_vel_controller'], 1])
#!/usr/bin/env python
import rospy
import rosservice
import time
if __name__ == "__main__":
while(True):
rosservice.call_service("/move_base/clear_costmaps", service_args=None)
time.sleep(2)
