def handle_auction_server_callback(auction_req):
global role_assigned, actual_role
global master_server, buyer_server
# update number of messages in parameter server
if rospy.has_param('/num_messages'):
num_messages = rospy.get_param('/num_messages')
num_messages += 2
rospy.set_param('/num_messages', num_messages)
# Clean up
if rospy.has_param('/auction_closed'):
if rospy.get_param('/auction_closed') == True:
role_assigned = False
rospy.loginfo(rospy.get_name()+' '+str(role_assigned))
# avoid node to take another role
if not role_assigned:
role_assigned = True
if auction_req.role == 'be_auctioneer':
return auctioneer.handle_auction_server_callback(auction_req)
elif auction_req.role == 'be_buyer':
return buyer_k_saap.handle_auction_server_callback(auction_req)
else:
return {'response_info':'invalid role requested'}
else:
return {'response_info':'node already have a role'}
def __init__(self):
rospy.set_param('/nav_experiments/topics', ['/base_controller/command'])
self.tf = tf.TransformListener()
self.target_frame = '/base_footprint'
self.pub = rospy.Publisher('/goal_state', PolygonStamped, latch=True)
self._as = actionlib.SimpleActionServer('/move_base', MoveBaseAction, execute_cb=self.execute_cb, auto_start=False)
self._as.start()
def handler(self, respawn_request):
node_requesting = respawn_request.node_name
faulty_port = respawn_request.port_name
#Get both assigned port numbers:
control_port = rospy.get_param('AER_Driver/control_port')
sensor_port = rospy.get_param('Sensor_Monitor/sensor_port')
#Get a list of actually available ports
actual_ports = glob.glob('/dev/ttyACM*')
if (len(actual_ports) > 1):
#Handle the respawn request
available_ports = []
for port_number in actual_ports:
if port_number != sensor_port:
available_ports.append(port_number)
#Make sure we have a port available
if (len(available_ports) == 0):
#If not, send an error code
return RespawnResponse(2)
#If we have available ports
kill_cmd = "rosnode kill /" + node_requesting
spawn_cmd = "rosrun AER " + node_requesting + ".py&"
#Kill requesting node
subprocess.call(kill_cmd, shell=True)
#Set new parameter
param_name = 'AER_Driver/control_port'
rospy.set_param(param_name, available_ports[0])
#Rerun the node
subprocess.call(spawn_cmd, shell=True)
else:
#Send an error code
return RespawnResponse(1)
def init():
global scaleStatusPub, camOdomPub,lisener,scaleOrbFlag,scale,odom_combined_tf
global velPub
rospy.init_node("orb_scale", anonymous=True)
scaleOrbFlag=rospy.get_param('/orb2base_scaleFlag',False)
if scaleOrbFlag:
#load scale value from scale.txt
file_path=os.path.split(os.path.realpath(__file__))[0]
fp3=open("%s/scale.txt"%(file_path),'r+')
for line in fp3:
value_list=line.split(" ")
scale=float(value_list[0])
if scale<=0.000001:
scale=0.
scaleOrbFlag=False
else:
rospy.set_param('/orb2base_scale',scale)
fp3.close
print "scale: "+str(scale)
lisener=tf.TransformListener()
odom_combined_tf=tf.TransformBroadcaster()
rospy.Subscriber("/system_monitor/report", Status, systemStatusHandler)
scaleStatusPub = rospy.Publisher('/orb_scale/scaleStatus', Bool , queue_size=5)
camOdomPub = rospy.Publisher("/ORB_SLAM/Odom", Odometry, queue_size=5)
rospy.Subscriber("/ORB_SLAM/Camera", Pose, cameraOdom)
velPub = rospy.Publisher('/cmd_vel', Twist , queue_size=5)
t=threading.Thread(target=publish_Tfs)
t.start()
def execute(self, userdata):
aux=userdata.current_robot_pose
aux.pose.position.x
aux.pose.position.y
yaw=userdata.current_robot_yaw
rospy.loginfo(OKGREEN+"I Have a new point"+ENDC)
if (userdata.objectOrientation== 'right') :
yaw=yaw+(math.pi/2)
if (userdata.objectOrientation== 'left'):
yaw=yaw-(math.pi/2)
if (userdata.objectOrientation== 'back'):
yaw=yaw-math.pi
if (userdata.objectOrientation=='front'):
yaw=yaw
value=["submap_0",userdata.objectName,aux.pose.position.x,
aux.pose.position.y,yaw]
rospy.loginfo(OKGREEN+str(value)+ENDC)
rospy.set_param("/mmap/poi/submap_0/"+str(userdata.objectName),value)
#"/restaurant/submap_0/"+str(userdata.objectName)
return 'succeeded'
def pause_bt_button_clicked(self):
'''
:param self:
:return:
'''
rospy.set_param('/apc/task_manager/running', False)
def Run(self):
if not self.sss.parse:
rospy.loginfo("Testing Sound modes...")
rospy.loginfo("If you can't hear something, check soundcard number (#card) with:")
rospy.loginfo(" cat /proc/asound/cards")
rospy.loginfo("Initialize card with:")
rospy.loginfo(" alsactrl init #card")
for i in range(1):
self.sss.say(["Hello, my name is Care-O-bot."])
self.sss.say(["Hello, my name is Care-O-bot.","How are you?"])
self.sss.say("sent00")
self.sss.say("sent00","de")
self.sss.say(123)
self.sss.say([123])
self.sss.say(["Hello, my name is Care-O-bot.","How are you?"],False)
self.sss.sleep(1)
self.sss.say(["This is a non blocking voice."])
rospy.set_param("script_server/sound/language","de")
self.sss.play("grasp_tutorial_01")
rospy.set_param("script_server/sound/language","en")
self.sss.play("grasp_tutorial_01")
def __init__(self):
self.json_file = rospy.get_param('~json', None)
self.is_apc2016 = rospy.get_param('~is_apc2016', True)
self.gripper = rospy.get_param('~gripper', 'gripper2016')
self.max_weight = rospy.get_param('~max_weight', -1)
if self.json_file is None:
rospy.logerr('must set json file path to ~json')
return
data = json.load(open(self.json_file))
self.work_order = {}
work_order_list = []
for order in data['work_order']:
bin_ = order['bin'].split('_')[1].lower()
self.work_order[bin_] = order['item']
work_order_list.append({'bin': bin_, 'item': order['item']})
rospy.set_param('~work_order', work_order_list)
self.object_data = None
if self.is_apc2016:
self.object_data = jsk_apc2016_common.get_object_data()
self.bin_contents = jsk_apc2016_common.get_bin_contents(param='~bin_contents')
self.msg = self.get_work_order_msg()
self.pub_left = rospy.Publisher(
'~left_hand', WorkOrderArray, queue_size=1)
self.pub_right = rospy.Publisher(
'~right_hand', WorkOrderArray, queue_size=1)
rospy.Service('~update_target', UpdateTarget, self._update_target_cb)
self.updated = False
rospy.Timer(rospy.Duration(rospy.get_param('~duration', 1)), self._timer_cb)
def execute(self, userdata=None):
# Store current base position
base_loc = self._robot.base.get_location()
base_pose = base_loc.frame
# Create automatic side detection state and execute
self._robot.speech.speak("I am now going to look for the table", block=False)
automatic_side_detection = AutomaticSideDetection2(self._robot)
side = automatic_side_detection.execute({})
self._robot.head.look_at_standing_person()
self._robot.speech.speak("The {} is to my {}".format(self._location_id, side))
self._robot.head.cancel_goal()
if side == "left":
base_pose.M.DoRotZ(math.pi / 2)
elif side == "right":
base_pose.M.DoRotZ(-math.pi / 2)
# Add to param server
loc_dict = {'x': base_pose.p.x(), 'y': base_pose.p.y(), 'phi': base_pose.M.GetRPY()[2]}
rospy.set_param("/restaurant_locations/{name}".format(name=self._location_id), loc_dict)
self._visualize_location(base_pose, self._location_id)
self._robot.ed.update_entity(id=self._location_id, frame_stamped=FrameStamped(base_pose, "/map"),
type="waypoint")
return "done"
def __init__(self, object_list, is_debug=False):
self._is_reload = rospy.get_param('/pr2_pbd_interaction/isReload')
self._exp_number = None
self._selected_step = 0
self._object_list = object_list
if (is_debug):
self._exp_number = rospy.get_param(
'/pr2_pbd_interaction/experimentNumber')
self._data_dir = self._get_data_dir(self._exp_number)
if (not os.path.exists(self._data_dir)):
os.mkdir(self._data_dir)
else:
self._get_participant_id()
rospy.set_param('data_directory', self._data_dir)
self.actions = dict()
self.current_action_index = 0
if (self._is_reload):
self._load_session_state(object_list)
rospy.loginfo("Session state loaded.")
n_actions = dict()
for k in self.actions.keys():
n_actions[str(k)] = self.actions[k].n_frames()
self._state_publisher = rospy.Publisher('experiment_state',
ExperimentState)
rospy.Service('get_experiment_state', GetExperimentState,
self.get_experiment_state_cb)
self._update_experiment_state()
def callback_stop(dat):
global listen_active
listen_active= not dat.data
rospy.set_param('/sense/stt/get_listen_active',listen_active)
pubActive = rospy.Publisher('/act/robot/set_listen_led', Bool)
pubActive.publish(listen_active)
rospy.loginfo("listen active:"+str(listen_active))
def main():
"""
Run the main loop, by instatiating a PendulumSimulator, and then
calling ros.spin
"""
rospy.init_node('pendulum_simulator') # , log_level=rospy.INFO)
# check what the value of the links param is
links_bool = rospy.get_param('links', False)
if not rospy.has_param('links'):
rospy.set_param('links', links_bool)
count = 1
try:
system = create_systems(2, link_length='1.0', link_mass='2.0', frequency='0.5', amplitude='0.5', damping='0.1')
user = User(USER_NUMBER, MAX_TRIALS, STARTING_TRUST, ALPHA, INPUT_DEVICE)
r = rospy.Rate(100)
while user.current_trial_num < user.max_num_trials:
rospy.loginfo("Trial number: %d", count)
sim = PendulumSimulator(system, user.current_trust, user.current_trial_num)
while not sim.finished:
r.sleep()
rospy.loginfo("current trust: %f", user.current_trust)
user.update_trust(sim.task_trust, sim.tcost)
rospy.loginfo("new current trust: %f, task trust: %f", user.current_trust, sim.task_trust)
del sim
count = count + 1
time.sleep(2)
except rospy.ROSInterruptException:
pass
rospy.loginfo('Session Complete')
def __init__(self):
self.indexHtmlTemplate = Template(filename="templates/indexTemplate.html")
self.lang = rospy.get_param("/system_lang", "en")
# self.lang='en'
# Load default dict
fp = open("lang/" + self.lang + ".txt", "r")
self.language = json.load(fp, "utf-8")
fp.close()
# Load specific dict if different to english
if self.lang != "en":
try:
fp = open("lang/" + self.lang + ".txt", "r")
# self.language.update(json.load(fp))
self.language.update(json.load(fp))
fp.close()
except IOError:
print "Language error"
self.lang = "en"
# Post the system lang to ROS and set the Param
if self.lang == "en" or self.lang == "es" or self.lang == "it":
rospy.set_param("/system_lang", self.lang)
lang_pub = rospy.Publisher("/system_lang", String)
lang_pub.publish(String(self.lang))
print "PUBLISHING NEW LANG: " + self.lang
"""
def privateParam(param_name, default_value=None):
response = rospy.get_param("~" + param_name, default_value)
if response == None:
raise Exception("ERROR: Mandatory '%s' parameter not found."%(param_name))
else:
rospy.set_param("~" + param_name, response)
return response
def Run(self):
if not self.sss.parse:
rospy.loginfo("Testing Sound modes...")
for i in range(1):
self.sss.say(["Hello, my name is Care-O-bot."])
self.sss.say(["Hello, my name is Care-O-bot.","How are you?"])
self.sss.say("sent00")
self.sss.say("sent00","de")
self.sss.say(123)
self.sss.say([123])
self.sss.say(['Once Upon A Time, there was a Shoemaker named Zerbo. He very carefully handcrafted every pair of shoes. Each pair of shoes was made especially for each person, and they were made to fit perfectly.'],False)
self.sss.sleep(1)
self.sss.say(["This is a non blocking voice which should be played in parallel to the previous text."])
rospy.set_param("script_server/sound/language","de")
self.sss.play("grasp_tutorial_01")
rospy.set_param("script_server/sound/language","en")
self.sss.play("grasp_tutorial_01")
def listener():
rospy.init_node('vp_ardrone2_joy_node')
rospy.loginfo("vp_ardrone2_joy - OK!")
if not rospy.has_param("/vp_ardrone2/flag2"):
rospy.set_param("/vp_ardrone2/flag2",0)
sub = rospy.Subscriber("joy",Joy,controller)
rospy.spin()
def __init__(self, hostname, username, password, interface):
self.hostname = hostname
self.username = username
self.password = password
self.interface = interface
self.passmgr = urllib2.HTTPPasswordMgrWithDefaultRealm()
self.passmgr.add_password(realm=None,
uri="http://%s/"%(hostname),
user=username,
passwd=password)
self.auth_handler = urllib2.HTTPBasicAuthHandler(self.passmgr)
self.opener = urllib2.build_opener(self.auth_handler)
urllib2.install_opener(self.opener)
self.current_config = {}
self.get_current_config()
self.set_req_args = {}
self.set_req_args["action"] = "Apply"
self.set_req_args["submit_type"] = ""
self.set_req_args["change_action"] = ""
self.set_req_args["commit"] = "1"
self.set_req_args["next_page"] = ""
self.avail_txpower_list = self.get_avail_txpower_list()
node_name = rospy.get_name()
for param_name in self.current_config:
param_full_name = node_name + "/" + param_name
if not rospy.has_param(param_full_name):
rospy.set_param(param_full_name, self.current_config[param_name])
def kinect_cb(self,rawDepthImage):
# If data is requested for localization
if(rospy.get_param('visual_tracker')==rospy.get_name()):
self.cloud= read_points(rawDepthImage, field_names=None, skip_nans=False, uvs=[])
# Convert a cloud generator to a list
self.points = []
for pt in self.cloud:
pt = list(pt)
pt.append(1)
self.points.append(pt)
nearestPoint=self.getNearestPoint()
print nearestPoint[2]
# Create a new message to be sent to ROSToArduino
messageToROS=fromObserver_msg()
messageToROS.observerID=rospy.get_name()
messageToROS.coordinates=(nearestPoint)
self.KinectToROS_pub.publish(messageToROS)
# Reset the visual_tracker parameter
rospy.set_param('visual_tracker','')
def controller(data):
# проверка takeoff - кнопка start/10
if(data.buttons[9]==1): # взлет
rospy.loginfo("взлет")
pub1=rospy.Publisher('ardrone/takeoff', Empty)
pub1.publish()
time.sleep(1)
elif(data.buttons[8]==1): # посадка- кнопка back/9
rospy.loginfo("посадка")
pub1=rospy.Publisher('ardrone/land', Empty)
pub1.publish()
time.sleep(1)
elif(data.buttons[5]==1): # посадка- кнопка RB/6
rospy.loginfo("посадка")
pub1=rospy.Publisher('ardrone/land', Empty)
pub1.publish()
rospy.set_param("/vp_ardrone2/flag2",1)
time.sleep(1)
elif(data.buttons[4]==1): # flattrim- кнопка RB/5
rospy.loginfo("flattrim")
rospy.set_param("/vp_ardrone2/flag2",1)
serv1=rospy.ServiceProxy('ardrone/flattrim',std_srvs.srv.Empty)
res1=serv1();
else: # другие клавиши
pass
def __init__(self):
# create all required variables:
self.running_flag = False
self.ref_time = None
self.ref_state = None
# self.ref_func = None
self.ref_arr = None
self.tbase = rospy.Time.now()
self.X = None
self.first_time = True
self.get_intergral_time = True
self.time_gap = 0
self.Einx = 0
self.Einy = 0
# create all publishers and subscribers:
self.cmd_pub = rospy.Publisher("cmd_vel_raw", Twist, queue_size=1)
#set the transfrom broadcaster to publish the velocity transfrom from spatial frame to body frame
self.br = tf.TransformBroadcaster()
self.timer = rospy.Timer(rospy.Duration(0.005), self.timercb)
#intialize the subscriber to subscribe the data from odom/, this odom/ is used as feedback for driving the base
self.odom_sub = rospy.Subscriber("odom", Odometry, self.odomcb, queue_size=10)
self.traj_sub = rospy.Subscriber("base_controller/follow_joint_trajectory/goal", FollowJointTrajectoryActionGoal,
self.trajcb, queue_size=1)
rospy.set_param('return_to_base', False)
return
def launch_heartbeat_operator_node():
rospy.init_node('dronemap_heartbeat_operator', anonymous=True)
# get parameters from the ros parameters server
platform_ip_address = rospy.get_param('/cloud_platform_ip_address')
platform_port_number = rospy.get_param('/cloud_platform_port_number')
buffer_size = rospy.get_param('/buffer_size')
rate = rospy.Rate(50)
if not rospy.has_param("/drone_registered"):
rospy.set_param("/drone_registered", False)
while not rospy.get_param("/drone_registered"):
rospy.loginfo(rospy.get_caller_id() + ": Still not registered yet.")
rate.sleep()
print "yes, registered"
# get the formatter
formatter = create_message_formatter()
# start the operator
client = TCPClient("dronemap_heartbeat_operator", platform_ip_address, platform_port_number, formatter, buffer_size)
HeartbeatControlOperator(client)
# Keep python from exiting until this node is stopped
rospy.spin()
def odom_res_changed(self, value):
self.settings.odom_res_slider.blockSignals(True)
self.settings.odom_res_slider.setValue(int(100*value))
self.settings.odom_res_slider.blockSignals(False)
self.settings.odom_res_box.setValue(value)
self.robot.odometer.res = value
rospy.set_param('~encoder_resolution', value)
def gps_noise_changed(self, value):
self.settings.gps_noise_slider.blockSignals(True)
self.settings.gps_noise_slider.setValue(int(100*value))
self.settings.gps_noise_slider.blockSignals(False)
self.settings.gps_noise_box.setValue(value)
self.robot.gps.noise = value
rospy.set_param('~gps_noise', value)
def __init__(self):
rospy.init_node("enemy_manager")
self.ns = rospy.get_namespace()
self.launch = roslaunch.scriptapi.ROSLaunch()
self.launch.start()
names = ['t1', 't2', 't3']
self.nodes = {}
self.processes = {}
y = 0
for name in names:
y += 1
param_name = "/" + name + "/y"
if self.ns != "/":
param_name = self.ns + param_name
rospy.set_param(param_name, y)
self.nodes[name] = roslaunch.core.Node("grid_defense", "enemy.py",
name=name, namespace=self.ns,
args="")
self.processes[name] = self.launch.launch(self.nodes[name])
# while not rospy.is_shutdown():
# rospy.sleep(1.0)
rospy.spin()
for name in names:
rospy.loginfo("stopping " + name)
self.processes[name].stop()
def __init__(self):
# find USB rockets and set them up
manager = RocketManager()
manager.acquire_devices()
if len(manager.launchers) == 0:
rospy.logerr("Failed to find rocket launcher")
exit(-1)
# TODO: handle more than one launcher
self.launcher = manager.launchers[0]
# launcher.usb_debug = True
# Calibration
if rospy.has_param('avg'):
print 'Calibration found on prameter server'
self.avg = rospy.get_param('avg')
self.var = rospy.get_param('var')
else:
print 'No calibration found. calibrating'
(self.avg, self.var) = self.calibrate()
rospy.set_param('avg', self.avg)
rospy.set_param('var', self.var)
self.pos_home()
self.alpha_target = ALPHA_HOME
self.beta_target = BETA_HOME
self.firing = False
self.moving = False
rospy.Service('rocket_fire', Empty, self.fire)
rospy.Subscriber('rocket_command', RocketCommand, self.position)
self.pub = rospy.Publisher('rocket_position', RocketPosition)
rospy.logdebug('rocket_driver ready')
def __init__(self):
self.bInitialized = False
# initialize
self.name = 'acquirevoltages2msg'
rospy.init_node(self.name, anonymous=False)
self.nodename = rospy.get_name()
self.namespace = rospy.get_namespace()
# Load the parameters.
self.params = rospy.get_param('%s' % self.nodename.rstrip('/'), {})
self.defaults = {'channels_ai':[0,1,2,3,4,5,6,7],
'channels_di':[0,1,2,3,4,5,6,7]}
SetDict().set_dict_with_preserve(self.params, self.defaults)
rospy.set_param('%s' % self.nodename.rstrip('/'), self.params)
self.command = None
# Messages & Services.
self.topicAI = '%s/ai' % self.namespace.rstrip('/')
self.pubAI = rospy.Publisher(self.topicAI, MsgAnalogIn)
self.topicDI = '%s/di' % self.namespace.rstrip('/')
self.pubDI = rospy.Publisher(self.topicDI, MsgDigitalIn)
self.subCommand = rospy.Subscriber('%s/command' % self.nodename.rstrip('/'), String, self.command_callback, queue_size=1000)
self.get_ai = rospy.ServiceProxy('get_ai', SrvPhidgetsInterfaceKitGetAI)
self.get_di = rospy.ServiceProxy('get_di', SrvPhidgetsInterfaceKitGetDI)
rospy.sleep(1) # Allow time to connect.
self.bInitialized = True
def run(self):
while 1:
try:
path,val = self.queue.get(block=True, timeout=1.0/self._freq)
with rosgobject.get_parameter_lock():
rospy.set_param(path,val)
except Queue.Empty:
pass
with rosgobject.get_parameter_lock():
try:
val = rospy.get_param(self._path)
has_param = True
except KeyError:
val = None
has_param = False
create_param = False
if has_param:
if self._val is None:
self._existscallback(self._path)
if val != self._val:
self._changecallback(self._path, val)
self._val = val
elif self._create is not None:
with rosgobject.get_parameter_lock():
rospy.set_param(self._path, self._create)
create_param = True
if create_param:
self._log.info("set parameter %s to %r" % (self._path, self._create))
self._existscallback(self._path)
self._changecallback(self._path, self._create)
def gyro_noise_changed(self, value):
self.settings.gyro_noise_slider.blockSignals(True)
self.settings.gyro_noise_slider.setValue(int(100*value))
self.settings.gyro_noise_slider.blockSignals(False)
self.settings.gyro_noise_box.setValue(value)
self.robot.gyroscope.noise = math.radians(value)
rospy.set_param('~gyro_noise', math.radians(value))
def go_to_start(self, midpoint, pub):
r = 10.0
rate = rospy.Rate(r)
for i in range(0, 15 * int(r)):
out_msg = QuadPositionDerived()
out_msg.x = 0.0
out_msg.y = 0.0
out_msg.z = 0.6
out_msg.yaw = 0
out_msg.x_vel = 0.0
out_msg.y_vel = 0.0
out_msg.z_vel = 0.0
out_msg.yaw_vel = 0
out_msg.x_acc = 0.0
out_msg.y_acc = 0.0
out_msg.z_acc = 0.0
out_msg.yaw_acc = 0
pub.publish(out_msg)
rate.sleep()
target_point = [0.0, 0.0, 0.0]
target_point[0] = midpoint[0] + self.radius
target_point[1] = midpoint[1]
target_point[2] = midpoint[2]
outpos = self.transform_coordinates(target_point, self.theta)
rospy.set_param("trajectory_generator/start_point", [0.0, 0.0, 0.6])
rospy.set_param("trajectory_generator/end_point", outpos)
sl_gen = StraightLineGen()
sl_gen.generate()
return outpos
def set_windowsize(n=10):
"""
Set the size of the history to maintain for each sensor
:param n: The number of values to keep
:return: None
"""
rospy.set_param('aggregation_windowsize', n)
def launchWrapper(self, virtual_robot, ros_namespace, frequency=200):
"""
Launches the ROS wrapper
Parameters:
virtual_robot - The instance of the simulated model
ros_namespace - The ROS namespace to be added before the ROS topics
advertized and subscribed
frequency - The frequency of the ROS rate that will be used to pace
the wrapper's main loop
"""
if MISSING_IMPORT is not None:
raise pybullet.error(MISSING_IMPORT)
self.virtual_robot = virtual_robot
self.ros_namespace = ros_namespace
self.frequency = frequency
rospy.init_node("qibullet_wrapper",
anonymous=True,
disable_signals=False)
# Upload the robot description to the ros parameter server
try:
if isinstance(self.virtual_robot, PepperVirtual):
robot_name = "pepper"
elif isinstance(self.virtual_robot, NaoVirtual):
robot_name = "nao"
elif isinstance(self.virtual_robot, RomeoVirtual):
robot_name = "romeo"
else:
raise pybullet.error(
"Unknown robot type, wont set robot description")
package_path = roslib.packages.get_pkg_dir("naoqi_driver")
urdf_path = package_path + "/share/urdf/" + robot_name + ".urdf"
with open(urdf_path, 'r') as file:
robot_description = file.read()
rospy.set_param("/robot_description", robot_description)
except IOError as e:
raise pybullet.error("Could not retrieve robot descrition: " +
str(e))
# Launch the robot state publisher
robot_state_publisher = roslaunch.core.Node("robot_state_publisher",
"robot_state_publisher")
self.roslauncher = roslaunch.scriptapi.ROSLaunch()
self.roslauncher.start()
self.roslauncher.launch(robot_state_publisher)
# Initialize the ROS publisher and subscribers
self._initPublishers()
self._initSubscribers()
# Launch the wrapper's main loop
self._wrapper_termination = False
self.spin_thread = Thread(target=self._spin)
self.spin_thread.start()
def __init__(self):
self.node_name = 'cont_anti_instagram_node'
robot_name = rospy.get_param("~veh",
"") #to read the name always reliably
self.active = True
self.locked = False
# Initialize publishers and subsImportError: cannot import namecribers
self.pub_trafo = rospy.Publisher("~transform",
AntiInstagramTransform,
queue_size=1)
self.pub_trafo_CB = rospy.Publisher("~colorBalanceTrafo",
AntiInstagramTransform_CB,
queue_size=1)
self.pub_health = rospy.Publisher("~health",
AntiInstagramHealth,
queue_size=1,
latch=True)
self.pub_mask = rospy.Publisher("~mask", Image, queue_size=1)
self.pub_geomImage = rospy.Publisher("~geomImage", Image, queue_size=1)
self.sub_image = rospy.Subscriber(
'/ubiquityrobot/camera_node/image_raw/compressed',
CompressedImage,
self.cbNewImage,
queue_size=1)
# Verbose option
self.verbose = rospy.get_param('line_detector_node/verbose', False)
# Read parameters
self.interval = self.setupParameter("~ai_interval", 10)
self.fancyGeom = self.setupParameter("~fancyGeom", False)
self.n_centers = self.setupParameter("~n_centers", 6)
self.blur = self.setupParameter("~blur", 'median')
self.resize = self.setupParameter("~resize", 0.2)
self.blur_kernel = self.setupParameter("~blur_kernel", 5)
self.cb_percentage = self.setupParameter("~cb_percentage", 50)
self.trafo_mode = self.setupParameter("~trafo_mode", 'cb')
if not (self.trafo_mode == "cb" or self.trafo_mode == "lin"
or self.trafo_mode == "both"):
rospy.loginfo(
"cannot understand argument 'trafo_mode'. set to 'both' ")
self.trafo_mode == "both"
rospy.set_param(
"~trafo_mode",
"both") # Write to parameter server for transparancy
# rospy.loginfo("[%s] %s = %s " % (self.node_name, "~trafo_mode", "both"))
# Initialize health message
self.health = AntiInstagramHealth()
# Initialize transform message
self.transform = AntiInstagramTransform()
# initialize color balance transform message
self.transform_CB = AntiInstagramTransform_CB()
# initialize AI class
self.ai = AntiInstagram()
self.ai.setupKM(self.n_centers, self.blur, 1, self.blur_kernel)
# initialize msg bridge
self.bridge = CvBridge()
self.image_msg = None
# timer for continuous image process
self.timer_init = rospy.Timer(rospy.Duration(self.interval),
self.processImage)
rospy.loginfo('ai: Looking for initial trafo.')
# bool to switch from initialisation to continuous mode
self.initialized = False
self.max_it_1 = 10
self.max_it_2 = 2
# container for mask and maskedImage
self.mask255 = []
self.geomImage = []
self.cb_percentage = rospy.Timer(rospy.Duration.from_sec(1.0),
self.updateParams)
def setupParameter(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
rospy.set_param(param_name,
value) #Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
def main():
urdfname = "/data/ros/yue_ws/src/tcst_pkg/urdf/ur5.urdf"
filename = "/data/ros/yue_ws/src/tcst_pkg/yaml/cam_300_industry_20200518.yaml"
# urdfname="/data/ros/ur_ws/src/universal_robot/ur_description/urdf/ur5.urdf"
desiruv = []
lambda1 = -1.166666
detat = 0.05
z = 0.92
ace = 50
vel = 0.1
urt = 0
ratet = 10
p0 = IBVSControl(filename, lambda1, urdfname)
ur_reader = Urposition()
ur_sub = rospy.Subscriber("/joint_states", JointState, ur_reader.callback)
u_error_pub = rospy.Publisher("/feature_u_error", Float64, queue_size=10)
v_error_pub = rospy.Publisher("/feature_v_error", Float64, queue_size=10)
z_depth_pub = rospy.Publisher("/camera_depth", Float64, queue_size=10)
ur_pub = rospy.Publisher("/ur_driver/URScript", String, queue_size=10)
down_to_q = []
desire_joint_angular = [124.08, -86.65, 60.29, 295.00, -89.38, 166.73]
start_angular = [44.87, -70.67, 40.06, 299.46, -89.69, 182.71]
cartisian_feedback = p0.getpi(desire_joint_angular)
start_angular_back = p0.getpi(start_angular)
rate = rospy.Rate(ratet)
count_for_desire = 0
place_count = 2
while not rospy.is_shutdown():
desiruv = []
uvlist = []
open_ibvs_flag = rospy.get_param("open_ibvs_flag")
open_go_to_desire = rospy.get_param("open_go_to_desire")
print("open_ibvs_flag,open_go_to_desire", open_ibvs_flag,
open_go_to_desire)
# print(p0.uv_list_buffer)
if open_ibvs_flag == 1:
print("Object picking")
if len(p0.uv_list_buffer) != 0:
uvlist.append(p0.uv_list_buffer[-1])
desiruv.append([333, 241])
feature_error = p0.get_feature_error(desiruv, uvlist[0])
print "Ibvs is ok?---", p0.return_error_ok(
feature_error.tolist()[0][0],
feature_error.tolist()[0][1])
if p0.return_error_ok(feature_error.tolist()[0][0],
feature_error.tolist()[0][1]) == False:
print "feature error\n", feature_error
u_error_pub.publish(feature_error.tolist()[0][0])
v_error_pub.publish(feature_error.tolist()[0][1])
q_now = ur_reader.ave_ur_pose
detaangular = p0.get_deta_joint_angular(
detat, uvlist, z, desiruv, uvlist[0], q_now)
q_pub_now = p0.get_joint_angular(q_now, detaangular)
print "q_now\n", q_now
print "q_pub_now\n", q_pub_now
down_to_q = q_pub_now
ss = "movej([" + str(q_pub_now[0]) + "," + str(
q_pub_now[1]) + "," + str(q_pub_now[2]) + "," + str(
q_pub_now[3]) + "," + str(
q_pub_now[4]) + "," + str(
q_pub_now[5]) + "]," + "a=" + str(
ace) + "," + "v=" + str(
vel) + "," + "t=" + str(urt) + ")"
ur_pub.publish(ss)
if p0.return_error_ok(feature_error.tolist()[0][0],
feature_error.tolist()[0][1]) == True:
rospy.set_param("/open_go_to_object", 0)
# down_to_q=[0.6840241781464097, -1.7849443418136726, -1.2719627004551626, -1.671977857427553, 1.6020880964542237, 2.9842358676782488]
if len(down_to_q) != 0:
temp_q0 = p0.get_inverse_to_box(down_to_q, 0, 0, -0.15)
p0.move_ur_l(ur_pub, temp_q0, 0.2, ace, urt)
time.sleep(1)
x_length = +0.112
y_length = +0.04 #-0.005
z_depth = 0
temp_q1 = p0.get_inverse_to_box(
temp_q0, x_length, y_length, z_depth)
p0.move_ur_l(ur_pub, temp_q1, 0.2, ace, urt)
time.sleep(1)
x_length = +0.002
y_length = +0.002 #-0.005
depth = 0
temp_q2 = p0.get_inverse_to_box(
temp_q1, x_length, y_length, depth)
p0.move_ur_l(ur_pub, temp_q2, 0.2, ace, urt)
time.sleep(1)
temp_q3 = p0.get_inverse_to_box(temp_q2, 0, 0, -0.19)
p0.move_ur_l(ur_pub, temp_q3, 0.2, ace, urt)
time.sleep(1)
os.system(
"rostopic pub /io_state std_msgs/String '55C8010155' -1"
)
p0.move_ur_l(ur_pub, temp_q2, 0.2, ace, urt)
time.sleep(1)
p0.move_ur_l(ur_pub, cartisian_feedback, 0.2, ace, urt)
time.sleep(5)
rospy.set_param("open_ibvs_flag", 0)
rospy.set_param("/open_go_to_desire", 1)
uvlist = []
p0.uv_list_buffer = []
# if open_go_to_desire==1 and open_ibvs_flag==0:
# print("Desire pick",len(p0.uv_desire_list_buffer))
# if len(p0.uv_desire_list_buffer)!=0:
# # desire_object=p0.image_space_planning([569,474],[44,65],3,3)
# uvlist.append([p0.uv_desire_list_buffer[-1][0],p0.uv_desire_list_buffer[-1][1]])
# desiruv.append([333,241])
# # desiruv.append([550,339])
# feature_error=p0.get_feature_error(desiruv,uvlist[0])
# print "Ibvs is ok?---",p0.return_error_ok_desire(feature_error.tolist()[0][0],feature_error.tolist()[0][1])
# if p0.return_error_ok_desire(feature_error.tolist()[0][0],feature_error.tolist()[0][1])==False:
# print "feature error\n",feature_error
# u_error_pub.publish(feature_error.tolist()[0][0])
# v_error_pub.publish(feature_error.tolist()[0][1])
# q_now=ur_reader.ave_ur_pose
# detaangular=p0.get_deta_joint_angular(detat,uvlist, z, desiruv, uvlist[0], q_now)
# q_pub_now=p0.get_joint_angular(q_now,detaangular)
# print "q_now\n", q_now
# print "q_pub_now\n",q_pub_now
# down_to_q=q_pub_now
# ss = "movej([" + str(q_pub_now[0]) + "," + str(q_pub_now[1]) + "," + str(q_pub_now[2]) + "," + str(
# q_pub_now[3]) + "," + str(q_pub_now[4]) + "," + str(q_pub_now[5]) + "]," + "a=" + str(ace) + "," + "v=" + str(
# vel) + "," + "t=" + str(urt) + ")"
# # print ss
# ur_pub.publish(ss)
# if p0.return_error_ok_desire(feature_error.tolist()[0][0],feature_error.tolist()[0][1])==True:
# depth=-0.15
# temp_q=p0.get_inverse_to_box(down_to_q,0,0,depth)
# p0.move_ur_l(ur_pub,temp_q,0.2,ace,urt)
# time.sleep(1)
# if place_count>=4:
# pose_place=p0.caculate_place_pose_from_ref(temp_q,0)
# else:
# pose_place=p0.caculate_place_pose_from_ref(temp_q,1)
# rospy.set_param("/open_go_to_object",0)
# p0.move_ur_l(ur_pub,pose_place[0],0.2,ace,urt)
# time.sleep(1)
# p0.move_ur_l(ur_pub,pose_place[1],0.2,ace,urt)
# time.sleep(1)
# p0.move_ur_l(ur_pub,pose_place[place_count],0.2,ace,urt)
# time.sleep(1)
# temp_q_place=p0.get_inverse_to_box(pose_place[place_count],0,0,-0.44)
# p0.move_ur_l(ur_pub,temp_q_place,0.2,ace,urt)
# time.sleep(1)
# os.system("rostopic pub /io_state std_msgs/String '55C8010055' -1")
# # p0.move_ur_l(ur_pub,cartisian_feedback,0.2,ace,urt)#for debug
# p0.move_ur_l(ur_pub,pose_place[place_count],0.2,ace,urt)
# time.sleep(2)
# p0.uv_desire_list_buffer=[]
# rospy.set_param("open_go_desire_flag",0)
# p0.move_ur_l(ur_pub,start_angular_back,0.2,ace,urt)
# time.sleep(5)
# rospy.set_param("/open_go_to_desire",0)
# rospy.set_param("/open_go_to_object",1)
# rospy.set_param("open_ibvs_flag",1)
# place_count+=1
rate.sleep()
def main():
rospy.init_node('odom_reader', anonymous=True)
rospack = rospkg.RosPack() # Find rospackge locations
listener = tf.TransformListener()
with open(rospack.get_path('map_reader') + '/mapCoords.yaml', 'r') as f:
doc = yaml.load(f)
f.close()
arenaPnt1 = doc["arenaTopLeft"]
arenaPnt2 = doc["arenaBotRight"]
rospy.set_param("/arenaPnt1", arenaPnt1)
rospy.set_param("/arenaPnt2", arenaPnt2)
badSquare1 = doc["deadZoneTopLeft"]
badSquare2 = doc["deadZoneBotRight"]
rospy.set_param("/deadZone1", badSquare1)
rospy.set_param("/deadZone2", badSquare2)
rospy.set_param("/currentRobotR", 0.0)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
(trans, rotate) = listener.lookupTransform('/odom', '/base_link',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
euler = tf.transformations.euler_from_quaternion(rotate)
roboX = trans[0]
roboY = trans[1]
roboR = euler[2]
print roboX
print roboY
print roboR
rospy.set_param("/currentRobotX", roboX)
rospy.set_param("/currentRobotY", roboY)
rospy.set_param("/currentRobotR", roboR)
rate.sleep()
def run_single_evaluation(self, target_dir):
rospy.loginfo("Starting evaluation on target '%s'.", target_dir)
# Check target dir is valid (approximately)
if not os.path.isfile(os.path.join(target_dir, "data_log.txt")):
rospy.logerr(
"Invalid target directory: Could not find a 'data_log.txt' file."
)
return
# Check for rosbag renaming
self.eval_log_file = open(os.path.join(target_dir, "data_log.txt"),
'a+')
lines = [line.rstrip('\n') for line in self.eval_log_file]
if not "[FLAG] Rosbag renamed" in lines:
for line in lines:
if line[:14] == "[FLAG] Rosbag:":
file_name = os.path.join(os.path.dirname(target_dir),
"tmp_bags", line[15:] + ".bag")
if os.path.isfile(file_name):
os.rename(
file_name,
os.path.join(target_dir, "visualization.bag"))
self.writelog(
"Moved the tmp rosbag into 'visualization.bag'")
self.eval_log_file.write("[FLAG] Rosbag renamed\n")
else:
self.writelog("Error: unable to locate '" + file_name +
"'.")
rospy.logwarn("Error: unable to locate '" + file_name +
"'.")
self.eval_log_file.close() # Make it available for voxblox node
# Create meshes and voxblox eval
if self.create_meshes:
# Configure directory
if not os.path.isdir(os.path.join(target_dir, "meshes")):
os.mkdir(os.path.join(target_dir, "meshes"))
if self.evaluate or self.create_meshes or self.evaluate_volume:
# Set params and call the voxblox evaluator
rospy.set_param(self.ns_voxblox + "/target_directory", target_dir)
try:
self.eval_voxblox_srv()
except:
rospy.logerr(
"eval_voxblox service call failed. Shutting down.")
sys.exit(-1)
# Reopen logfile
self.eval_log_file = open(os.path.join(target_dir, "data_log.txt"),
'a+')
if self.create_plots:
# Create dirs
if not os.path.isdir(os.path.join(target_dir, "graphs")):
os.mkdir(os.path.join(target_dir, "graphs"))
if os.path.isfile(os.path.join(target_dir, "voxblox_data.csv")):
# Read voxblox data file
data_voxblox = self.read_voxblox_data(
os.path.join(target_dir, "voxblox_data.csv"))
if len(data_voxblox['RosTime']) > 1:
if 'MeanError' in data_voxblox:
self.plot_sim_overview(data_voxblox, target_dir)
else:
rospy.loginfo(
"Unevaluated 'voxblox_data.csv', skipping dependent graphs."
)
else:
rospy.loginfo(
"Too few entries in 'voxblox_data.csv', skipping dependent graphs."
)
else:
rospy.loginfo(
"No 'voxblox_data.csv' found, skipping dependent graphs.")
if os.path.isfile(os.path.join(target_dir, "performance_log.csv")):
# Read performance data file
data_perf = {}
headers = None
with open(os.path.join(target_dir,
"performance_log.csv")) as infile:
reader = csv.reader(infile,
delimiter=',',
quotechar='|',
quoting=csv.QUOTE_MINIMAL)
for row in reader:
if row[0] == 'RunTime':
headers = row
for header in headers:
data_perf[header] = []
continue
for i in range(len(row)):
data_perf[headers[i]].append(row[i])
# Create graph
if len(data_perf['RosTime']) > 1:
self.plot_perf_overview(data_perf, target_dir)
else:
rospy.loginfo(
"Too few entries in 'performance_log.csv', skipping dependent graphs."
)
else:
rospy.loginfo(
"No 'performance_log.csv' found, skipping dependent graphs."
)
if os.path.isfile(os.path.join(target_dir, "error_hist.csv")):
# Read error data file
with open(os.path.join(target_dir,
"error_hist.csv")) as infile:
reader = csv.reader(infile,
delimiter=',',
quotechar='|',
quoting=csv.QUOTE_MINIMAL)
data = np.array(list(reader))
data_error_hist = data[1:, 1:].astype(int)
# Create graph
self.plot_error_hist(data_error_hist, target_dir)
else:
rospy.loginfo(
"No 'error_hist.csv' found, skipping dependent graphs.")
# Finish
if self.clear_voxblox_maps:
# Remove all voxblox maps to free up disk space
shutil.rmtree(os.path.join(target_dir, 'voxblox_maps'),
ignore_errors=True)
self.eval_log_file.close()
def PickAndPlaceImpl(self, req):
collision = rospy.get_param("/Collision")
print("Collision was set to: {}".format(collision))
# if collision detected
if collision:
# ---------
print "Placing Down Object:" + req.object
print "Moving to {} PLACE APPROACH".format(req.object)
self.state = STATE.PLACING
rospy.sleep(.5)
if self.stop:
return
place_pose_offset = copy.deepcopy(self.object_place_poses)
place_pose_offset = place_pose_offset[req.object]
place_pose_offset.position.z = place_pose_offset.position.z + 0.2
self.moveToPose(place_pose_offset)
rospy.sleep(.5)
if self.stop:
return
# ---------
print "Moving to {} PLACE".format(req.object)
self.moveToPose(self.object_place_poses[req.object])
if self.stop:
return
self._gripper.command_position(100.0)
#self.state = STATE.PLACED
if self.stop:
return
rospy.sleep(1.0)
if self.stop:
return
# ---------
print "Moving to {} PLACE APPROACH".format(req.object)
place_pose_offset = copy.deepcopy(self.object_place_poses)
place_pose_offset = place_pose_offset[req.object]
place_pose_offset.position.z = place_pose_offset.position.z + 0.2
self.moveToPose(place_pose_offset)
if self.stop:
return
rospy.sleep(1.0)
self._gripper.command_position(0.0)
if self.stop:
return
self.state = STATE.PLACED
#self.state = STATE.IDLE
rospy.set_param("/Collision", False)
# otherwise run as normal
else:
#-----
# check if collision happened yet, stop if it did
collision = rospy.get_param("/Collision")
if collision:
rospy.set_param("/Collision", False)
# self.stop
return
#-----
if self.stop:
return
print "Picking UP Object: " + req.object
# self._limb.set_joint_position_speed(0.2)
if self.stop:
return
self.state = STATE.NEUTRAL
# ---------
'''print "Moving to {} APPROACH + Z".format(req.object)
rospy.sleep(1.0)
approach_pose_offset = copy.deepcopy(self.object_approach_poses[req.object])
approach_pose_offset.position.z = approach_pose_offset.position.z + 0.2;
approach_pose_offset.position.y = approach_pose_offset.position.y + 0.03;
self.moveToPose(approach_pose_offset)
if self.stop:
return
self._gripper.command_position(100.0)
self.state = STATE.APPROACHING
if self.stop:
return
rospy.sleep(1.0)
if self.stop:
return
# ---------
print "Moving to {} APPROACH".format(req.object)
rospy.sleep(1.0)
approach_pose_offset = copy.deepcopy(self.object_approach_poses[req.object])
approach_pose_offset.position.z = approach_pose_offset.position.z + 0.05;
approach_pose_offset.position.y = approach_pose_offset.position.y + 0.03;
self.moveToPose(approach_pose_offset)
if self.stop:
return
self.state = STATE.APPROACHING
if self.stop:
return
rospy.sleep(1.0)
if self.stop:
return'''
# ---------
#-----
# check if collision happened yet, stop if it did
collision = rospy.get_param("/Collision")
print("Collision was set to: {}".format(collision))
if collision:
rospy.set_param("/Collision", False)
# self.stop
return
#-----
print "Moving to {} PICK + Z".format(req.object)
self.state = STATE.PICKING
pick_pose_offset = copy.deepcopy(
self.object_pick_poses[req.object])
pick_pose_offset.position.z = pick_pose_offset.position.z + 0.2
pick_pose_offset.position.y = pick_pose_offset.position.y
self.moveToPose(pick_pose_offset)
self._gripper.command_position(100.0)
if self.stop:
return
self.state = STATE.PICKED
if self.stop:
return
rospy.sleep(.5)
if self.stop:
return
# ---------
#-----
# check if collision happened yet, stop if it did
collision = rospy.get_param("/Collision")
print("Collision was set to: {}".format(collision))
if collision:
rospy.set_param("/Collision", False)
# self.stop
return
#-----
if req.object == "sports_ball":
print "Moving to {} PICK".format(req.object)
self.state = STATE.PICKING
pick_pose_offset = copy.deepcopy(
self.object_pick_poses[req.object])
pick_pose_offset.position.z = pick_pose_offset.position.z - 0.06
pick_pose_offset.position.y = pick_pose_offset.position.y - 0.04
pick_pose_offset.position.x = pick_pose_offset.position.x + 0.04
self.moveToPose(pick_pose_offset)
if self.stop:
return
self._gripper.command_position(0.0)
self.state = STATE.PICKED
if self.stop:
return
rospy.sleep(.5)
if self.stop:
return
else:
print "Moving to {} PICK".format(req.object)
self.state = STATE.PICKING
pick_pose_offset = copy.deepcopy(
self.object_pick_poses[req.object])
pick_pose_offset.position.z = pick_pose_offset.position.z - 0.075
pick_pose_offset.position.y = pick_pose_offset.position.y - 0.095
pick_pose_offset.position.x = pick_pose_offset.position.x + 0.01
self.moveToPose(pick_pose_offset)
if self.stop:
return
self._gripper.command_position(0.0)
self.state = STATE.PICKED
if self.stop:
return
rospy.sleep(.5)
if self.stop:
return
# ---------
#-----
# check if collision happened yet, stop if it did
collision = rospy.get_param("/Collision")
print("Collision was set to: {}".format(collision))
if collision:
rospy.set_param("/Collision", False)
# self.stop
return
#-----
print "Moving to {} APPROACH".format(req.object)
rospy.sleep(.5)
pick_pose_offset = copy.deepcopy(
self.object_pick_poses[req.object])
pick_pose_offset.position.z = pick_pose_offset.position.z + 0.2
self.moveToPose(pick_pose_offset)
if self.stop:
return
self.state = STATE.APPROACHING
if self.stop:
return
rospy.sleep(.5)
if self.stop:
return
# ---------
#-----
# check if collision happened yet, stop if it did
collision = rospy.get_param("/Collision")
print("Collision was set to: {}".format(collision))
if collision:
rospy.set_param("/Collision", False)
# self.stop
return
#-----
print "Placing Down Object:" + req.object
print "Moving to {} PLACE APPROACH".format(req.object)
self.state = STATE.PLACING
rospy.sleep(.5)
if self.stop:
return
place_pose_offset = copy.deepcopy(self.object_place_poses)
place_pose_offset = place_pose_offset[req.object]
place_pose_offset.position.z = place_pose_offset.position.z + 0.2
self.moveToPose(place_pose_offset)
rospy.sleep(.5)
if self.stop:
return
# ---------
#-----
# check if collision happened yet, stop if it did
collision = rospy.get_param("/Collision")
print("Collision was set to: {}".format(collision))
if collision:
rospy.set_param("/Collision", False)
# self.stop
return
#-----
print "Moving to {} PLACE".format(req.object)
self.moveToPose(self.object_place_poses[req.object])
if self.stop:
return
self._gripper.command_position(100.0)
#self.state = STATE.PLACED
if self.stop:
return
rospy.sleep(1.0)
if self.stop:
return
# ---------
#-----
# check if collision happened yet, stop if it did
collision = rospy.get_param("/Collision")
print("Collision was set to: {}".format(collision))
if collision:
rospy.set_param("/Collision", False)
# self.stop
return
#-----
print "Moving to {} PLACE APPROACH".format(req.object)
place_pose_offset = copy.deepcopy(self.object_place_poses)
place_pose_offset = place_pose_offset[req.object]
place_pose_offset.position.z = place_pose_offset.position.z + 0.2
self.moveToPose(place_pose_offset)
if self.stop:
return
rospy.sleep(1.0)
self._gripper.command_position(0.0)
if self.stop:
return
self.state = STATE.PLACED #checking the link between state.placed and Work
def execute_cb(self, goal):
rospy.set_param("/tabletop_segmentation/x_filter_max",
goal.table_region.x_filter_max)
rospy.set_param("/tabletop_segmentation/x_filter_min",
goal.table_region.x_filter_min)
rospy.set_param("/tabletop_segmentation/y_filter_max",
goal.table_region.y_filter_max)
rospy.set_param("/tabletop_segmentation/y_filter_min",
goal.table_region.y_filter_min)
rospy.set_param("/tabletop_segmentation/z_filter_max",
goal.table_region.z_filter_max)
rospy.set_param("/tabletop_segmentation/z_filter_min",
goal.table_region.z_filter_min)
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
return False
####################################
# start executing the action steps #
####################################
object_list = self.execution_steps(goal)
if object_list:
self._result.clusters_list = object_list
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
else:
self._as.set_succeeded(self._result)
def shutdown():
rospy.set_param('/aggregator/inStateMachine', False)
def get_keyboard_command(key_msg):
global keyboard_command
keyboard_command = key_msg
# force other nodes to stop moving and wait when there is a keyboard
if not is_twist_empty(keyboard_command):
rospy.set_param("KEY", True)
def execute(self, userdata):
rospy.set_param('/aggregator/inNavigation', self.state)
return 'succeeded'
class Planning(object):
def __init__(self):
self.logPosition = []
self.logPositionAll = []
self.flagLogPosition = 0
self.flagIsLogging = False
self.indOptimalLast = 10
self.desireDirDrive = FORWARD # desireDirDrive是为了触发底层换挡
self.curDirDrive = self.desireDirDrive # curDirDrive是为了协调内部程序，如航向变换
self.flagIsShifting = 0
self.distLog = 0
self.countSingleLane = 0
self.countLaneFinish = 0
self.flagLaneFinish = 0
self.flagMCUIsShifting = 0
self.flagChangesPre = True
self.flagLogPointStart = True
self.pointStart = np.array([0,0])
self.newStraightPathFlag = False # 准备错距到新的直线碾压路径，用来合并换道规划路径
self.cloudTaskID = 0 # 云端接收到新任务，ID增1，通过此变量判断是否有新任务，有新任务后，重置相关标志位和地图
self.mapEndPoint, self.numGPSallPoint = findMapEndPoint(numAllLaneGLB,distBetweenLaneGLB) # (numAllLaneGLB,distOffsetStep=1.5)
np.save(os.path.join(cur_path,'../log/mapEndPoint.npy'),self.mapEndPoint)
self.indexCircleMap = 0
if mapTypeGLB == 'raw':
self.GPSmap = np.load(os.path.join(cur_path,'../map/rawMap.npy'))
else:
if taskTypeGLB == 'csdCircle':
genCurveMap()
self.GPSmap = np.load(os.path.join(cur_path,''.join(['../map/curveMap', str(self.indexCircleMap), '.npy'])))
elif taskTypeGLB == 'csdLine':
self.GPSmap = genMapPoint(self.mapEndPoint[self.countLaneFinish][0],self.mapEndPoint[self.countLaneFinish][1],self.numGPSallPoint)
while not flagPositionReceivedGLB:
print 'is waiting vehState topic'
if rospy.is_shutdown():
sys.exit()
time.sleep(0.02)
self.indLastClosest, self.flagClosestPointSearched = commonFcn.findClosestPointInit(recv, self.GPSmap)
self.indLastPre = self.indLastClosest # 检测是否换地图
self.dataPlanning = {'xCtrPath':[], 'yCtrPath':[], \
'xGlbPath':[], 'yGlbPath':[], \
'pathCandidate':np.array([[0,0],]), 'indOptimal':0, \
'modeDrive':NORMAL, 'vbrMode':0,'distObst':1000.}
self.distYClose = 0.
self.modeDrive = NORMAL
self.vbrMode = 0
self.prePoint = []
self.toCloudStartedFlag = False # 0: 未开始碾压任务； 1: 已按需开始云端任务
self.toCloudEndTaskFlag = False # 0: 任务未结束； 1: 当前碾压任务已完成
def calibrate(self):
# Calibrated Parameters
global CALIBRT
flagOpenCalib = rospy.get_param('~GLBflagOpenCalib') # 控制标定量接收程序
if flagOpenCalib == 1:
CALIBRT = rospy.get_param('~CALIBRT')
rospy.set_param('~GLBflagOpenCalib', 0)
# @timeit
def publish(self):
'''
'''
PathMsg = PathDisplay()
PathMsg.xGlbPath = self.dataPlanning['xGlbPath']
PathMsg.yGlbPath = self.dataPlanning['yGlbPath']
# different from the usage in std_msgs
dim = MultiArrayDimension()
dim.stride = int(self.dataPlanning['indOptimal'])
dim.size = self.dataPlanning['pathCandidate'].shape[0]
PathMsg.xLocPath.data = np.array(self.dataPlanning['pathCandidate'][:,0]).flatten()
PathMsg.xLocPath.layout.dim = [dim]
PathMsg.yLocPath.data = np.array(self.dataPlanning['pathCandidate'][:,1]).flatten()
PathMsg.yLocPath.layout.dim = [dim]
PathMsg.header.stamp = rospy.Time.now()
basePathPub.publish(PathMsg)
PlanMsg = PlanOP()
PlanMsg.prePoint.x = self.prePoint[0]
PlanMsg.prePoint.y = self.prePoint[1]
PlanMsg.prePoint.z = 0.
PlanMsg.xCtrPath = self.dataPlanning['xCtrPath']
PlanMsg.yCtrPath = self.dataPlanning['yCtrPath']
PlanMsg.distObst = self.dataPlanning['distObst']
PlanMsg.desireDirDrive = self.desireDirDrive
PlanMsg.curDirDrive = self.curDirDrive
PlanMsg.flagIsShifting = self.flagIsShifting
PlanMsg.distYClose = self.distYClose
PlanMsg.modeDrive = self.modeDrive
PlanMsg.vbrMode = self.vbrMode
PlanMsg.startedFlag = self.toCloudStartedFlag
PlanMsg.endTaskFlag = self.toCloudEndTaskFlag
PlanMsg.header.stamp = rospy.Time.now()
planPub.publish(PlanMsg)
@timeit
def update(self):
CAN_flagStart_trace = recv['start_trace']
modeDriveLane, vbrMode = self.changeDirection(recv)
self.vbrMode = vbrMode
indClosest, indPre, self.flagClosestPointSearched = commonFcn.findClosestPoint( \
recv, self.GPSmap, self.indLastClosest, CALIBRT['distPreview'], \
self.flagClosestPointSearched)
self.indLastClosest = indClosest
self.indLastPre = indPre
self.prePoint = self.GPSmap[indPre,:]
# 考虑找不到点的情况，此时停止规划
# if indClosest >= self.GPSmap.shape[0]-1:
if self.flagClosestPointSearched == False:
modeDriveSafe = STOP
rospy.logwarn('no point is available')
elif modeDriveLane == STOP:
# 防止到达终点时，无路点可用，导致polyfit等一系列函数报错
rospy.loginfo('is stoping and pausing planning')
else:
# try:
self.dataPlanning = self.localPlanning(indClosest,indPre,recv)
modeDriveSafe = self.dataPlanning['modeDrive']
# except Exception, e:
# modeDriveSafe = STOP_SAFE
# rospy.logerr('localPlanning run error')
# print 'repr(e):\t', repr(e)
# merge the modeDrive output
# CAN_flagStart_trace = 1
if modeDriveLane == STOP or CAN_flagStart_trace == False or \
recv['stopSafeArea'] == True or recv['cloudStartTask'] == False:
self.modeDrive = STOP
else:
if self.curDirDrive == BACK:
self.modeDrive = modeDriveSafe # 后退无避障功能
else:
self.modeDrive = modeDriveSafe #
# self.estimateDistErr(recv)
# print 'indClosest, indPre',indClosest, indPre
def replyCloudServer(self):
if recv['cloudTaskID'] > self.cloudTaskID:
# 说明有新任务
# 响应云端新任务，重置所有相关标志位和地图
# 更新任务结束标志位
if self.flagLaneFinish == 1:
self.toCloudEndTaskFlag = True
# 更新发送到云端的开始任务标志位
if self.toCloudEndTaskFlag == True:
self.toCloudStartedFlag = False # 复位任务开始标志位
else:
if self.modeDrive == NORMAL:
self.toCloudStartedFlag = True
self.cloudTaskID = recv['cloudTaskID']
# 检测正在起步
def checkStart(self,recv):
position = recv
point_self = np.asarray([position['x'],position['y']])
if np.abs(position['x']) < 0.1 and np.abs(position['y']) < 0.1:
pass
else:
if self.flagLogPointStart == True:
self.pointStart = np.array([position['x'],position['y']])
self.flagLogPointStart = False
self.flagChangesPre = True
distStart = np.linalg.norm(point_self-self.pointStart)
# 起步过渡过程结束
if distStart > 7.0:
self.flagChangesPre = False
# 计算实时偏差，采用点到直线距离
# 先排除初始化过程，未接收到定位数据的情况
def estimateDistErr(self,recv):
position = recv
if np.abs(position['x']) < 0.1 and np.abs(position['y']) < 0.1:
# is initializing
distErr = 0.
else:
startPoint,endPoint = self.mapEndPoint[self.countLaneFinish][0],self.mapEndPoint[self.countLaneFinish][1]
polyCoeff = np.polyfit([startPoint[0],endPoint[0]],[startPoint[1],endPoint[1]],1)
A_Coeff,C_Coeff = polyCoeff
B_Coeff = -1.
distErr = (A_Coeff*position['x']+B_Coeff*position['y']+C_Coeff) / np.sqrt(A_Coeff**2.+B_Coeff**2.)
print('distErr',distErr)
GLBlogDistErr.append(distErr)
self.distYClose = distErr
def changeDirection(self,recv):
CAN_flagIsShifting = recv['is_shifting']
shift_stat = recv['shift']
position = recv
modeDriveLane = NORMAL
point_self = np.asarray([position['x'],position['y']])
point_mapEnd = np.asarray([self.GPSmap[-1,0],self.GPSmap[-1,1]])
distToEnd = np.linalg.norm(point_self-point_mapEnd)
# 圆弧路径规划，不断延长地图
if taskTypeGLB == 'csdCircle':
if self.indLastPre >= self.GPSmap.shape[0]-2:
# 延长地图
self.indexCircleMap += 1
if self.indexCircleMap > 3:
self.indexCircleMap = 3
else:
GPS_all = np.load(os.path.join(cur_path,''.join(['../map/curveMap', str(self.indexCircleMap), '.npy'])))
# self.GPSmap = np.concatenate((self.GPSmap[self.indLastClosest:-1], GPS_all), axis=0)
self.GPSmap = np.concatenate((self.GPSmap, GPS_all), axis=0)
# 检测正在起步
# self.checkStart(recv)
# if distToEnd < CALIBRT['distIsEnd']:
# rospy.logwarn("have reach the path end, please stop")
distIsEnd = CALIBRT['distIsEnd'] # 5 # 正式在工地上使用错距模式
if self.curDirDrive == BACK:
distIsEnd = max(1., (CALIBRT['distIsEnd'] - 3.4))
if distToEnd < distIsEnd and self.flagIsShifting == 0:
if taskTypeGLB == 'csdCircle':
self.flagLaneFinish = 1
else:
# self.flagIsShifting == 0是为了避免换挡过程地图未切换时一直满足CALIBRT['distIsEnd']
self.countSingleLane += 1
claCurSinglwLane = countSingleLane -1 # 以线进行判断
if claCurSinglwLane // 8 == 1 or claCurSinglwLane // 8==2 or claCurSinglwLane// 8==3 or claCurSinglwLane//8==4 :
self.vbrMode = 1
if self.countSingleLane >= timesSingleLaneGLB: #指示几趟
self.countSingleLane = 0
self.countLaneFinish += 1
self.newStraightPathFlag = True
if self.countLaneFinish >= numAllLaneGLB:
self.flagLaneFinish = 1
self.countLaneFinish = numAllLaneGLB-1 # -1的目的：避免后面程序索引溢出
if self.flagLaneFinish == 0: # 走完全程停车，则不必变换方向和地图
self.flagIsShifting = 1 # start shift
# 变换期望的换挡方向
if self.desireDirDrive == FORWARD:
self.desireDirDrive = BACK
else:
self.desireDirDrive = FORWARD
#判断是否正在换挡
if self.flagIsShifting == 1:
# # 加self.flagIsShifting == 1是为了避免完全由底层控制，但终点时，无法进入这段程序
flagShiftDone = False # init
if vehicle_model.name == 'A60':
if (self.desireDirDrive == BACK and shift_stat == shift_R) \
or (self.desireDirDrive == FORWARD and shift_stat == shift_D) : # 表明换挡完成
flagShiftDone = True
else:
if CAN_flagIsShifting >= 1: # 0 1; '>=' is for uncertain float
# #第一次判断，认为底层响应了换挡信号
self.flagMCUIsShifting = 1
if self.flagMCUIsShifting==1 and CAN_flagIsShifting==False: # should: True-->0
flagShiftDone = True
if flagShiftDone == True:
self.flagIsShifting = 0 # 换挡完成后，flagIsShifting恢复成0
self.flagMCUIsShifting = 0
# 表明换挡完成
self.flagLogPointStart = True
# self.mapEndPoint同时控制车道和方向。
# 车道由第一个索引控制
# 第二个索引 0 1互换，则表示起点终点互换，方向切换
if self.flagLaneFinish == 0: # 走完全程停车，则不必变换方向和地图
self.indLastClosest = 0 # 换地图前重新初始化
self.curDirDrive = self.desireDirDrive
if not mapTypeGLB == 'raw':
if self.curDirDrive == FORWARD:
self.GPSmap = genMapPoint(self.mapEndPoint[self.countLaneFinish][0],self.mapEndPoint[self.countLaneFinish][1],self.numGPSallPoint)
elif self.curDirDrive == BACK:
self.GPSmap = genMapPoint(self.mapEndPoint[self.countLaneFinish][1],self.mapEndPoint[self.countLaneFinish][0],self.numGPSallPoint)
# 记录跟踪实际坐标点
if self.flagLogPosition == 0: # 到终点记录一次并保存后，便不再记录
if self.curDirDrive == BACK:
self.flagIsLogging = True
self.logPosition.append(point_self)
# 保存记录的跟踪坐标点，并清空为再次记录做准备
if (self.curDirDrive == FORWARD or self.flagLaneFinish == 1) and self.flagIsLogging == True: # 逻辑是刚走完BACK，马上切换到FORWARD或是终点
# 此逻辑不足之处在于，必须走完当前路线，才能保存数据
self.flagIsLogging = False
self.logPositionAll.append(self.logPosition)
self.logPosition = []
# 停车模式判断
if (self.flagLaneFinish==1) | (self.flagIsShifting==1): # self.flagLaneFinish只有再次初始化才能清除，意味着车到这只有停车模式
modeDriveLane = STOP
self.countChangesPre = 0
print 'tttttttt',self.flagLaneFinish
# 用于驾校测试，消除停车积分
if vehicle_model.name == 'A60':
if shift_stat == shift_P or shift_stat == shift_N:
modeDriveLane = STOP
# 保存实际走过的位置点
# 全程走完时，计算所有线路回程的跟踪误差。去程因存在规划偏移，故无意义
if self.flagLaneFinish == 1 and self.flagLogPosition == 0:
self.flagLogPosition = 1
np.save(os.path.join(cur_path,'../log/logPositionAll.npy'),self.logPositionAll)
vbrMode = self.vbrMode
return modeDriveLane,vbrMode
def checkObstacle(self,pathCandidate):
#################################################################
#通过寻找第一个弧线上第一个栅格为0（障碍物）来确定无障碍弧长
#弧长越长越好
#################################################################
if self.curDirDrive == FORWARD:
OccupancyValueMat = recv['OccupancyValueForward']
else:
OccupancyValueMat = recv['OccupancyValueBackward']
#障碍物检测横向区域，左右各widthMargin*0.1m 。 校园大圈：11；驾校：22;工程车23,左右总宽4.6m,振动碾对角宽6.6m
widthMargin = int(CALIBRT['widthObstDetect']/0.1)
limitNumOccupied = 1 #认定为不可通行时占据栅格数目限值
x_S = pathCandidate[0]
y_S = pathCandidate[1]
indRow = np.minimum(299,np.round(np.array(x_S)/0.1).astype(np.int32)) # 校园大圈：284； 驾校:290
indRow = np.maximum(0,indRow)
indRowMat = np.tile(indRow,(2*widthMargin+1,1)).transpose()
indCol = np.minimum(200,100+np.round(np.array(y_S)/0.1).astype(np.int32)) # 负的在右边
indColTemp = np.arange(-widthMargin,widthMargin+1,1)
indColTemp1 = np.tile(indColTemp,(indCol.shape[0],1))
indColMatTemp = indColTemp1+indCol.reshape(-1,1)
indColMat = np.minimum(200,indColMatTemp)
indColMat = np.maximum(0,indColMat)
awardOccupancyTemp = OccupancyValueMat[indRowMat,indColMat] # x_S行，2*widthMargin+1列
checkFree = np.where(awardOccupancyTemp>80) # > 60表示有障碍物
occupiedRowTemp = np.array(checkFree)[0]
occupiedRow = np.unique(occupiedRowTemp) # 删除数组中由不同列导致的相同行号
numOccupiedRow = occupiedRow.size
closedObstaclDist = 1000. # 初始值无穷大，表示无障碍物
if numOccupiedRow <= limitNumOccupied: #无障碍
awardOccupancy = 1
else:
# awardOccupancy = checkFree[0][0] # 循环行数，可以抽象等价为路径长度
awardOccupancy = 0
'''
障碍物距离计算
note: awardOccupancyTemp是OccupancyValueMat的子集, 所以不能直接通过它索引x轴距离。
需按如下方法：
首先通过occupiedRow找到障碍物在awardOccupancyTemp中的行数，然后根据这个
行数，映射到indRowMat矩阵的真实物理意义行数。
'''
if self.curDirDrive == BACK:
distOffset = DIST_OBST_OFFSET_BACK
else:
distOffset = DIST_OBST_OFFSET_FORWARD
closedObstaclDist = (indRowMat[np.min(occupiedRow),0])*0.1 - distOffset
return awardOccupancy,closedObstaclDist
# @timeit
def genCandidate(self,d0,df,wayPointLocal):
'''
generate local path candidate
Coordinate:
vehicle local coordinate
Parameter:
wayPointLocal: wayPoint in local coordinate, two row
'''
if not taskTypeGLB == 'campus':
pathCandidate = commonFcn.bezierBased(df, wayPointLocal)
else:
pathCandidate = commonFcn.laneBased(d0,df,wayPointLocal)
# 障碍物检测，输入车辆坐标系序列点
awardOccupancy,closedObstaclDist = self.checkObstacle(pathCandidate)
result = [pathCandidate,awardOccupancy,closedObstaclDist]
return result
# @timeit
def localPlanning(self,indClosest,indPre,position):
global GLBlogDistErr
global CALIBRT, countCALIBRTGLB
global CenterGlobalEndGLB
flagCandidateAllDie = 0
safeDist2RightEdge = 0.5
modeDrive = NORMAL
xCenterGlobal = np.array(self.GPSmap[indClosest:indPre+1,0])
yCenterGlobal = np.array(self.GPSmap[indClosest:indPre+1,1])
# 路径终点，采用之前的路点，避免各种拟合函数错误
distCenterPathVect = np.array([self.GPSmap[indPre,0:2] - \
self.GPSmap[indClosest,0:2]])
distCenterPath = np.linalg.norm(np.array([distCenterPathVect]))
# if distCenterPath < max(5., CALIBRT['distIsEnd']):
# xCenterGlobal, yCenterGlobal = CenterGlobalEndGLB
# else:
# CenterGlobalEndGLB = np.array([xCenterGlobal, yCenterGlobal])
# 抽样中心线
# numSample = xCenterGlobal.size
# indexSample = np.arange(0,numSample,int(numSample/100)+1)
# xCenterGlobal = xCenterGlobal[indexSample]
# yCenterGlobal = yCenterGlobal[indexSample]
# bezier平滑中心线，结果发现，转弯处偏离过多
# wayPoint = np.array([xCenterGlobal,yCenterGlobal])
# xCenterGlobal, yCenterGlobal = commonFcn.bezierSmooth(wayPoint)
# spline平滑中心线(1)或单纯增加道路点(0)，从而可以使全局地图可以更稀疏，加快最近点搜索速度
# 经验证此种增加数据点的方法失效，另寻差值方法
# numGenPathPoint = max(100, int(CALIBRT['distPreview'] / 0.2))
# spl = UnivariateSpline(xCenterGlobal,yCenterGlobal)
# spl.set_smoothing_factor(0.)
# xCenterGlobal = np.linspace(xCenterGlobal[0],xCenterGlobal[-1],numGenPathPoint)
# yCenterGlobal = spl(xCenterGlobal)
# 估计当前位置横向偏差
# 原理：在最近点处，利用点斜式构造切线方程，然后用点到直线方程求距离
xClosest = self.GPSmap[indClosest,0]
yClosest = self.GPSmap[indClosest,1]
polyCoeffCenter = np.polyfit(xCenterGlobal,yCenterGlobal,2)
polyder1CoeffCenter = np.polyder(polyCoeffCenter) #导函数多项式系数
polyder1Center = np.polyval(polyder1CoeffCenter,xClosest) # 斜率
polyCoeffLine = [polyder1Center, yClosest-polyder1Center*xClosest] # A C
point = [position['x'],position['y']]
self.distYClose = commonFcn.calcDistPoint2Line(polyCoeffLine,point)
# 判断当前点在中心线左右，决定初始偏差正负
if indClosest >= self.GPSmap.shape[0]-2: # 避免索引溢出
S = 0
else:
PA = np.array([xClosest, yClosest])
PB = np.array(self.GPSmap[indClosest+1,0:2])
S = commonFcn.checkSidePoint2Line(PA, PB, point)
self.distYClose *= np.sign(S)
# 平滑中心线
# yCenterGlobal = np.polyval(polyCoeffCenter, xCenterGlobal)
# 先将最近的全局坐标转换为局部坐标
pointGlobal = np.array([xCenterGlobal, yCenterGlobal])
wayPointLocal = commonFcn.global_2_local_vect(position,pointGlobal)
##########################################################
#采用map函数代替for循环，构造不同df的 path candidate
dfMax = 2. # 依据道路宽度3.5m设置
dfStep = 0.1 # candidate采样距离偏差
d0 = self.distYClose # 规划时刻当前位置偏差
if not taskTypeGLB == 'campus':
'''
出山店遇到障碍物直接停车，不规划绕行
'''
dfMapTemp = np.array([0]) #不进行路径规划，只有一条candidate
else:
dfMapTemp = np.arange(-dfMax-1.,dfMax+dfStep+1.,dfStep)
dfMap = dfMapTemp.tolist()
numCandidate = len(dfMap)
d0Map = [d0]*numCandidate
wayPointLocalMap = [wayPointLocal]*numCandidate
candidate = map(self.genCandidate, d0Map,dfMap,wayPointLocalMap) # 为一个多维list
pathCandidate = [m for m,n,p in candidate] # 仍然是多维list
awardOccupancy = [n for m,n,p in candidate]
ObstaclDist = [p for m,n,p in candidate]
closedObstaclDist = np.min(ObstaclDist)
# 检查是否可通行
indHalfOptimal = np.where(np.array(awardOccupancy)>0)[0] # 索引数组
dimIndHalfOptimal = np.array(indHalfOptimal).size
defaultCandi = int(len(dfMap)/2) # 默认直接跟踪中线
xCtrPath = np.array([])
yCtrPath = np.array([])
indOptimal = defaultCandi
print 'safe number', dimIndHalfOptimal
if dimIndHalfOptimal == 0:
flagCandidateAllDie = 1 # 有障碍物，不能通行
xCtrPath = xCenterGlobal
yCtrPath = yCenterGlobal
else:
# 先找出最右的path，作为右边界。也就是最后一位索引path-->dfSafe[-1]
dfSafe = dfMapTemp[indHalfOptimal] # df_map是list不能用此方法索引
# 然后找到距离右边界安全距离的path
#左手坐标系
# dfCandi = dfSafe[-1]-1.
# indOptimalTemp = bisect.bisect_left(dfSafe,dfCandi) # numpy.searchsorted()
# 右手坐标系
# idea1
dfCandi = dfSafe[0]+1.
indOptimalTemp = bisect.bisect_right(dfSafe,dfCandi) # numpy.searchsorted()
# indOptimalTemp = np.argmin(costAll) #
# idea2
indOptimalTemp = int(len(indHalfOptimal)/2)
indOptimal = indHalfOptimal[indOptimalTemp] #通过上一步索引，反推剩下一半，也即是全局索引
# indOptimal = defaultCandi # 直接跟踪中线，只作用于显示
pointOptimal = pathCandidate[indOptimal]
xOptimalGlb, yOptimalGlb = commonFcn.local_2_global_vect(position,pointOptimal)
# select path to control module, according to option that choose local planning or not.
if vehicle_model.locPlan == True:
# use local planning path
xCtrPath = xOptimalGlb
yCtrPath = yOptimalGlb
else:
# use map directly
xCtrPath = xCenterGlobal
yCtrPath = yCenterGlobal
# 模式判断
# 首先处理大曲率转弯时，缩短预瞄距离
distPreviewCurv = CALIBRT['distPreview']
distPreviewReplan = CALIBRT['distPreview']
# if recv['flagBigCurv'] == True:
# distPreviewCurv = 9.
if flagCandidateAllDie == 1:
if closedObstaclDist < CALIBRT['distObstDetect']:
print("closedObstaclDist is ", closedObstaclDist, ' m')
# rospy.logwarn("due to obstacle, there is no path, please stop")
modeDrive = STOP_SAFE
else:
# 减速，并缩短规划路径，重规划
# modeDrive = DECELERATE
modeDrive = NORMAL # CSD
countCALIBRTGLB += 5
distPlanLag = -1. # 规划周期内行驶距离
distPreviewReplan = CALIBRT['distObstDetect'] + distPlanLag
else:
# 恢复预瞄距离
if countCALIBRTGLB > 0:
countCALIBRTGLB -= 1
if countCALIBRTGLB == 0:
CALIBRT = rospy.get_param('~CALIBRT')
modeDrive = NORMAL
CALIBRT['distPreview'] = min(distPreviewCurv, distPreviewReplan, CALIBRT['distPreview'])
pathCandidate = np.array(pathCandidate)
result = {'xCtrPath':xCtrPath, 'yCtrPath':yCtrPath, \
'xGlbPath':xCenterGlobal, 'yGlbPath':yCenterGlobal, \
'pathCandidate':pathCandidate, 'indOptimal':indOptimal, \
'modeDrive':modeDrive, 'distObst':closedObstaclDist}
return result
def getOccupancyGridForward(data):
recv['OccupancyValueForward'] = np.array(data.data).reshape(data.info.height,data.info.width).T
def getOccupancyGridBackward(data):
recv['OccupancyValueBackward'] = np.array(data.data).reshape(data.info.height,data.info.width).T
def getCAN(data):
recv['is_shifting'] = data.is_shifting
recv['start_trace'] = data.start_trace
def getVehState(data):
global flagPositionReceivedGLB
flagPositionReceivedGLB = True
recv['x'] = data.x
recv['y'] = data.y
recv['z'] = data.z
recv['roll'] = data.roll
recv['pitch'] = data.pitch
recv['azimuth'] = data.azimuth
recv['speed'] = data.speed
recv['brake_stat'] = data.brake_stat
recv['shift'] = data.shift
def getSteerAngle(data):
recv['curvature'] = data.curvature
recv['flagBigCurv'] = data.flagBigCurv
def getSafeAreaWarn(data):
recv['stopSafeArea'] = data.stopSafeArea
def getCloudTask(data):
recv['cloudTaskID'] = data.taskID
recv['cloudStartTask'] = data.startTask
recv['cloudPathType'] = data.VPathType
# recv['cloudFlagPoint'] = data.taskID
if __name__ == '__main__':
rospy.init_node('local_planning', anonymous = False)
recv = {'x':0., 'y':0., 'z':0., 'roll':0., 'pitch':0., 'azimuth':0., \
'speed':0., 'brake_stat':0, 'shift':shift_P, \
'is_shifting':False, 'start_trace':START_TRACE_INIT, \
'OccupancyValueForward':np.zeros([300,201]), \
'OccupancyValueBackward':np.zeros([300,201]), \
'curvature':0.000001, 'flagBigCurv':False, 'stopSafeArea':False, \
'cloudTaskID':0, 'cloudStartTask':False, 'cloudPathType':0, 'vbrMode':0}
# Loading Parameters
# Constant Parameters
param_file = cur_path + '/../params/paramLWB.yaml'
with open(param_file,'r') as f:
param = yaml.load(f)
mapTypeGLB = param["map"]["mapType"]
taskTypeGLB = param["map"]["taskType"]
if mapTypeGLB == 'raw' or taskTypeGLB == 'csdCircle':
# 校园正常测试，而不是模拟出山店
numAllLaneGLB = 1
distBetweenLaneGLB = 1. # 可随意设置，不再用到
timesSingleLaneGLB = 1
else:
numAllLaneGLB = param["map"]["numAllLane"]
distBetweenLaneGLB = param["map"]["distBetweenLane"]
timesSingleLaneGLB = param["map"]["timesSingleLane"]
# initialize Calibrated Parameters
rospy.set_param('~GLBflagOpenCalib', 0) # 控制标定量接收程序,初始化将yaml中的强制覆盖
# CALIBRT = rospy.get_param('~CALIBRT')
param_file = cur_path + '/../params/calibParam.yaml'
with open(param_file,'r') as f:
param = yaml.load(f)
CALIBRT = param["local_planning"]["CALIBRT"]
rospy.Subscriber("vehState", VehState, getVehState)
rospy.Subscriber("/forward/final_grid", OccupancyGrid, getOccupancyGridForward)
rospy.Subscriber("/backward/final_grid", OccupancyGrid, getOccupancyGridBackward)
rospy.Subscriber("/mcu_flag", mcuFlag, getCAN)
rospy.Subscriber('steerAngleCmd', stm32TX, getSteerAngle)
rospy.Subscriber('safeArea', PlanOP, getSafeAreaWarn)
rospy.Subscriber('cloudChatter',cloudTask, getCloudTask)
basePathPub = rospy.Publisher('basePath', PathDisplay, queue_size =1)
planPub = rospy.Publisher('planOutput', PlanOP, queue_size =5)
state = Planning()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
state.calibrate()
state.update()
state.publish()
rate.sleep()
np.save(os.path.join(cur_path,'../log/logDistErr.npy'),GLBlogDistErr)
rospy.spin() #rospy.spin()作用是当节点停止时让python程序退出，和C++ spin的作用不同
def genBlocksWorld(self):
"""Generates a world of exclusive-owned colored blocks.
Creates n_agents+1 clusters of objects, each of which is clustered
by color, position, proximity, or any combination.
Agent IDs start at 1, and colors are procedurally named as
'color0', 'color1', etc.
"""
n_agents = rospy.get_param("blocks_world/n_agents", 3)
n_objects = rospy.get_param("blocks_world/n_objects", 20)
# Variables to cluster blocks by
cluster_vars = rospy.get_param("blocks_world/cluster_vars",
['color', 'position',
'proximity', 'time'])
# Distance paramaters
avatar_rng = rospy.get_param("blocks_world/avatar_rng", [0.4, 0.8])
cluster_rng = rospy.get_param("blocks_world/cluster_rng", [0.4, 0.8])
cluster_rad = rospy.get_param("blocks_world/cluster_rad", 0.3)
object_rng = rospy.get_param("blocks_world/object_rng", [-1.0, 1.0])
# Color names
color_names = rospy.get_param("blocks_world/color_names",
["none", "red", "green", "blue"])
# Avg interaction periods (how frequently objects are interacted with)
beta_owned = rospy.get_param("blocks_world/beta_owned", 10.0)
beta_unowned = rospy.get_param("blocks_world/beta_unowned", 1000.0)
# Store scenario generation time
t_now = rospy.Time.now()
# Update color database
for c_id in range(n_agents + 1):
if c_id < len(color_names):
c_name = color_names[c_id]
else:
c_name = "color" + str(c_id)
color_names.append(c_name)
rospy.set_param("colors/" + c_name, [[0,0,0],[0,0,0]])
# Generate agents and their avatars
for i in range(n_agents):
av = Object(id=self.avt_base_id+i, is_avatar=True)
ag = Agent(id=i+1, avatar_id=av.id)
dist = r.uniform(*avatar_rng)
angle = r.uniform(i, i+1) * 2 * math.pi / n_agents
av.position = Point(x=dist*math.cos(angle) + self.home_pos.x,
y=dist*math.sin(angle) + self.home_pos.y,
z=self.ground_lvl)
self.agent_db[ag.id] = ag
self.object_db[av.id] = av
# Generate cluster centers
if 'proximity' in cluster_vars:
# Centers are avatar locations if clustering by proximity
centers = ([Point(self.home_pos.x,
self.home_pos.y,
self.ground_lvl)] +
[av.position for av in self.object_db.values()])
else:
# Otherwise, just cluster based on absolute position
centers = []
for i in range(n_agents + 1):
dist = r.uniform(*cluster_rng)
angle = r.uniform(i, i+1) * 2 * math.pi / (n_agents + 1)
c = Point(x=dist*math.cos(angle) + self.home_pos.x,
y=dist*math.sin(angle) + self.home_pos.y,
z=self.ground_lvl)
centers.append(c)
# Generate objects within each cluster
for i in range(n_objects):
obj = Object(id=self.obj_base_id+i)
c_id = i % (n_agents + 1) # Cluster ID, with 0 unowned
# Set membership in block category to 1
obj.categories["block"] = 1.0
# Generate object locations
if ('position' in cluster_vars or 'proximity' in cluster_vars):
# Cluster around centers
r_offset = r.uniform(0, cluster_rad)
r_angle = r.uniform(0, 2*math.pi)
c = centers[c_id]
obj.position = Point(x=c.x + r_offset * math.cos(r_angle),
y=c.y + r_offset * math.sin(r_angle),
z=c.z)
else:
# Uniform distribution if no position or proximity clustering
obj.position = Point(x=r.uniform(*object_rng),
y=r.uniform(*object_rng),
z=self.ground_lvl)
# Generate object colors
if 'color' in cluster_vars:
# Associate cluster with color
obj.color = color_names[c_id]
else:
# Choose color uniformly at random
col_id = r.choice(range(n_agents+1))
obj.color = color_names[col_id]
# Generate last interaction times according to exp. distribution
if 'time' in cluster_vars:
for a_id in range(1, n_agents+1):
if a_id == c_id:
t_last_action = np.random.exponential(beta_owned)
else:
t_last_action = np.random.exponential(beta_unowned)
t_last_action = rospy.Duration(t_last_action)
obj.t_last_actions[a_id] = t_now - t_last_action
# Assign ownership if not unowned
if c_id > 0:
obj.ownership[c_id] = 1.0
self.object_db[obj.id] = obj
def execute(self, userdata):
rospy.set_param('/aggregator/inVisionTracking', self.state)
return 'succeeded'
def setParam(self, name, value):
rospy.set_param(self.prefix + "/" + name, value)
self.updateParamsService([name])
def execute(self, userdata):
rospy.set_param('/aggregator/inStateMachine', self.state)
return 'succeeded'
'--target-network-update-freq',
type=int,
default=10,
help="freq (in training examples) in which to flush core network to"
" target network")
parser.add_argument(
'--target-network-update-coeff',
type=float,
default=1.0,
help="affine coeff for target network update. 0 => no update,"
" 0.5 => mean of core/target, 1.0 => clobber target completely")
opts = parser.parse_args()
print "OPTS", opts
# push refreshable args to param server
rospy.set_param("/q_table_policy/discount", opts.q_discount)
rospy.set_param("/q_table_policy/learning_rate", opts.q_learning_rate)
rospy.set_param("/q_table_policy/state_normalisation_squash",
opts.q_state_normalisation_squash)
rospy.set_param("/q_table_policy/summary_log_freq", opts.summary_log_freq)
rospy.set_param("/q_table_policy/target_network_update_freq",
opts.target_network_update_freq)
# build policy
NUM_ACTIONS = 3 # TODO: shared with sim
if opts.policy == "Baseline":
policy = policy.baseline.BaselinePolicy()
elif opts.policy == "DiscreteQTablePolicy":
policy = policy.discrete_q_table.DiscreteQTablePolicy(
num_actions=NUM_ACTIONS)
elif opts.policy == "NNQTablePolicy":
def setParams(self, params):
for name, value in params.iteritems():
rospy.set_param(self.prefix + "/" + name, value)
self.updateParamsService(params.keys())
def startCloudRecognition(self):
#Get path to the local cloud data base
object_path = roslib.packages.get_pkg_dir(
'qbo_webi'
) + "/config/" + "cloud_objects_" + self.language["current_language"]
#Check if the objects folder exists and if not, create it
if not os.path.exists(object_path):
os.makedirs(object_path)
rospy.loginfo("Qbo Webi: Path for the cloud's objects: " + object_path)
#If version file does not exists
if not os.path.isfile(object_path + "/version"):
#Download latest version
rospy.loginfo(
"Qbo Webi: Downloading latest version from the Q.bo Cloud")
resp = self.download_latest_version(object_path)
if resp != "ok":
return json.dumps({
'status':
'Unable to download latest version from Q.bo Cloud',
'object': 'none'
})
else:
#Read version file
version_str = ""
try:
version_file = open(object_path + "/version")
version_str = version_file.read().strip()
version_file.close()
except IOError as e:
return json.dumps({
'status': 'Could not access local files',
'object': 'none'
})
#Get latest version of Q.bo Cloud
rospy.loginfo(
"Qbo Webi: Getting version number from Q.bo Cloud...")
resp = self.get_version()
if resp == "":
return json.dumps({
'status': 'Could not get version number from Q.bo Cloud',
'object': 'none'
})
rospy.loginfo("Qbo Webi: Latest version is " + resp +
" and local version is " + version_str)
if resp == version_str: #If you have latest version, don't do anything
rospy.loginfo(
"Qbo Webi: Local version is the latest version: " +
version_str)
else:
rospy.loginfo(
"Qbo Webi: Downloading latest version from Q.bo Cloud")
#download latest version
resp = self.download_latest_version(object_path)
if resp != "ok":
return json.dumps({
'status':
'Unable to download latest version from Q.bo Cloud',
'object': 'none'
})
rospy.loginfo(
"Qbo Webi: Latest version has been succesfully downloaded from Q.bo Cloud"
)
#Store the previous update_path
update_path_back = roslib.packages.get_pkg_dir(
'qbo_object_recognition') + "/objects/objects_db/"
rospy.loginfo("Qbo Webi: Storing the previous update path: " +
update_path_back)
#Change the update_path of the object recognizer
rospy.set_param("/qbo_object_recognition/update_path", object_path)
rospy.loginfo(
"Qbo Webi: Loading object recognition with the cloud object path")
#Update the objects folder of the object recognizer to the latest version of Q.bo cloud
rospy.loginfo("Qbo Webi: Waiting for service update...")
rospy.wait_for_service("/qbo_object_recognition/update")
rospy.loginfo("Qbo Webi: Update service is ready!")
update_srv = rospy.ServiceProxy('/qbo_object_recognition/update',
Update)
resultUpdating = update_srv()
#Call the recognize service of the object recognizer
rospy.loginfo(
"Qbo Webi: Started object recognizer mode. Waiting for service...")
rospy.wait_for_service(
"/qbo_object_recognition/recognize_with_stabilizer")
rospy.loginfo("Qbo Webi: Recognize service is ready!")
recog = rospy.ServiceProxy(
'/qbo_object_recognition/recognize_with_stabilizer',
RecognizeObject)
object_recog = recog()
#Restore the previous update_path and load the previous object daba base
rospy.set_param("/qbo_object_recognition/update_path",
update_path_back)
rospy.loginfo(
"Restoring the object recognition node with the previous data base"
)
update_srv = rospy.ServiceProxy('/qbo_object_recognition/update',
Update)
resultUpdating = update_srv()
output = json.dumps({
'status': 'ok',
'object': str(object_recog.object_name)
})
return output
launch.launch(virtual_sensor_node)
except rospy.exceptions.ROSException:
print('Object Spawning Error')
pass
try:
launch.spin()
except rospy.exceptions.ROSInterruptException:
# After Ctrl+C is pressed.
pass
if __name__ == '__main__':
# Specify the initial sensor poses and target poses here.
# Simplified pose format: [x,y,theta].
# The number of sensors and targets to use is automatically determined by the dimensions of poses passed in.
sensor_poses=[
# [1,1,-3.04],\
[1,1.5,-3.04],\
[1,0.5,-3.04]]
target_poses = [[2.4, 4.5, 3.14 / 2]]
noise_level = input('Additive Gaussian Noise Std(a non-negative number):')
rospy.set_param('noise_level', float(noise_level))
# Specify the path to a basis launch file. It usually contains information about the .world file.
# Here we use the empty world launch file provided by gazebo_ros.
launch_simulation(sensor_poses=sensor_poses, target_poses=target_poses)
#Give time for everything to start up
if args.less_wait:
time.sleep(10)
else:
if args.debug:
time.sleep(
27
) #spawning a bunch of xterms for debugging takes longer than subprocesses
else:
time.sleep(17)
#End If different physical genome
print('Loading received genome into ros param and setting ready msg')
# Load the data into a parameter in ROS
rospy.set_param('rover_genome', data['genome'])
# Send a ready message on the topic to the behavior node
pub.publish(std_msgs.msg.Empty())
print('Done! Entering sleep onto sim evaluation is complete.')
# Wait for a result to return from the basicbot node
# TODO: Remove this spinning while loop with a different construct.
# Dependent upon the internal structure of ROS nodes!
# TODO: get the dependent software crash reset working wihtout having to send back a -1 result
this_eval_start = datetime.datetime.now()
while evaluation_result == '':
current_time = datetime.datetime.now()
if (current_time -
this_eval_start).total_seconds() > max_single_sim_running_time:
def cbCameraInfo(self):
self.imgWidth = rospy.get_param("/raspicam/width")
self.imgHeight = rospy.get_param("/raspicam/height")
rospy.set_param("/raspicam/vFlip", True)
def retrieve(listener,
br,
isExecute=True,
objId='expo_eraser',
binId=0,
withPause=False,
viz_pub=None,
recorder=None,
ws_object=None):
liftoff_pub = rospy.Publisher('/liftoff_time',
std_msgs.msg.String,
queue_size=10,
latch=False)
def compose_output():
return {
'collision': collision,
'grasp_possible': grasp_possible,
'plan_possible': plan_possible,
'execution_possible': execution_possible,
'gripper_opening': gripper_opening,
'graspPose': graspPose,
'gelsight_data': gelsight_data,
'final_object_pose': final_object_pose
}
q_initial = ik.helper.get_joints()
bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
spatula_tip_to_tcp_dist = rospy.get_param(
"/gripper/spatula_tip_to_tcp_dist")
l1 = 0.0
l2 = 0.0
l3 = spatula_tip_to_tcp_dist
tip_hand_transform = [l1, l2, l3, 0, 0, 0]
delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking')
delta_vision_pose[3:7] = np.array([0, 1, 0, 0])
vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3])
vision_pos[2] = 0.17786528
plans = []
plans_grasping = []
graspPose = []
plan_possible = False
execution_possible = False
gripper_opening = 0.0
grasp_possible = True
gelsight_data = []
collision = False
final_object_pose = None
rospy.set_param('is_record', False)
rospy.set_param('is_contact', False)
# ~10. Move to a location above the bin
plan, qf, plan_possible = generatePlan(q_initial,
vision_pos,
delta_vision_pose[3:7],
tip_hand_transform,
'fastest',
plan_name='retrieve_object')
if plan_possible:
plans_grasping.append(plan)
q_initial = qf
else:
return compose_output()
if isExecute and plan_possible:
#Publish liftoff time
liftoff_msgs = std_msgs.msg.String()
liftoff_msgs.data = 'Liftoff (Robot command)'
liftoff_pub.publish(liftoff_msgs)
#Execute non-guarded grasp plan move
executePlanForward(plans_grasping, withPause)
#~Check if picking success
low_threshold = 0.0035
high_threshold = 0.027
#shake robot
rospy.sleep(1.5)
if gripper.getGripperopening() > high_threshold and isExecute:
# We initialize drop listener
ws_object.start_listening_for_drop(bin_num=binId)
ik.ik.shake()
rospy.sleep(2.)
# We stop drop listener
ws_object.stop_listening_for_drop()
### We stop recording
if recorder is not None:
recorder.stop_recording(save_action=True)
lasers.stop(binId)
if isExecute and plan_possible:
rospy.sleep(2)
gripper_opening = gripper.getGripperopening()
if gripper_opening > high_threshold:
rospy.loginfo('[Picking] ***************')
rospy.loginfo('[Picking] Pick Successful (Gripper Opening Test)')
rospy.loginfo('[Picking] ***************')
execution_possible = True #temporary hack
else:
rospy.loginfo('[Picking] ***************')
rospy.loginfo('[Picking] Pick Inconclusive (Gripper Opening Test)')
rospy.loginfo('[Picking] ***************')
execution_possible = None
elif not isExecute:
execution_possible = plan_possible
return compose_output()
def _copy_to_parameter_server(self):
for param in extract_params(self.type.config_description):
rospy.set_param(self._prefix + param['name'],
self.config[param['name']])
def sliderKpRepVel(self):
position = self.horizontalSlider_kp_r_vel.sliderPosition()
rospy.set_param('swarm_node/local_plan/kp_repulsive_vel',
(position / 50.0))
self.lineEdit_kp_r_vel.setText(
str(rospy.get_param('swarm_node/local_plan/kp_repulsive_vel')))
def place(listener,
br,
isExecute=True,
binId=0,
withPause=False,
rel_pose=None,
BoxBody=None,
place_pose=None,
viz_pub=None,
is_drop=True,
recorder=None):
#########################################################
# fhogan and nikhilcd
# Description:
#~2017 picking primitive
#
#~Usage
#~ pick(objInput, listener, br, isExecute = True, objId = 'cheezit_big_original', flag = 1, withPause = False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None):
#
#~Parameters:
#
#~Inputs:
# 1) objInput: Input from vision or RViz (list of 12, or 7)
# 2) listener: Input from vision or RViz (list of 12, or 7)
# 3) br: Input from vision or RViz (list of 12, or 7)
# 4) isExecute: execute the robot motion if possible or not? (Bool)
# 5) objId: name of object (str)
# 6) binId: bin in which we need to place the objects. (It is useless when flag 0 and 1)
# 7) flag: Type of action directed by planning (0:run picking, 1:go to home (camera), 2: drop the object at target location)
# 8) withPause: Pause between robot motions (Bool)
# 9) rel_pose: object pose relative to link_6
# 10) BoxBody: Bounding box points resolved in link 6 frame
# 11) place_pose: desired position of bounding box
# 12) viz_pub: desired position of bounding box
#
#~Output:
# Dictionary of following keys:
# 1) grasp_possible: Does the object fit in the hand? (True/False)
# 2) plan_possible: Has the plan succeeded? (True/False)
# 3) execution_possible: Is there anything in the grasp? (True/False)
# 4) gripper_opening: gripper opening after grasp attempt (in meters)
# 5) graspPose: pose of the gripper at grasp (7X1 - position and quaternion)
# 6) gelsight_data: [] , dummy for now
#########################################################
#rospy.logdebug(msg, *args)
#rospy.loginfo(msg, *args)
#rospy.logwarn(msg, *args)
#rospy.logerr(msg, *args)
#rospy.logfatal(msg, *args)
###############################
## Pring input variables for ##
###############################
rospy.logdebug('[Picking] withPause %s', withPause)
rospy.logdebug('[Picking] rel_pose %s', rel_pose)
rospy.logdebug('[Picking] BoxBody %s', BoxBody)
rospy.logdebug('[Picking] place_pose %s', place_pose)
########################
## Initialize values ##
########################
q_initial = ik.helper.get_joints()
bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
spatula_tip_to_tcp_dist = rospy.get_param(
"/gripper/spatula_tip_to_tcp_dist")
delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking')
vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3])
plans = []
plans_grasping = []
plans_grasping2 = []
plans_guarded = []
plans_guarded2 = []
plans_placing = []
plans_placing2 = []
graspPose = []
plan_possible = False
execution_possible = False
gripper_opening = 0.0
grasp_possible = True
gelsight_data = []
collision = False
final_object_pose = None
rospy.set_param('is_record', False)
rospy.set_param('is_contact', False)
liftoff_pub = rospy.Publisher('/liftoff_time',
std_msgs.msg.String,
queue_size=10,
latch=False)
def compose_output():
return {
'collision': collision,
'grasp_possible': grasp_possible,
'plan_possible': plan_possible,
'execution_possible': execution_possible,
'gripper_opening': gripper_opening,
'graspPose': graspPose,
'gelsight_data': gelsight_data,
'final_object_pose': final_object_pose
}
#########################
## Constant parameters ##
#########################
UpdistFromBinFast = 0.15 # Margin above object during "move to a location"
UpdistFromBinGuarded = 0.05 # Margin above object during "move to a location"
gripperOriHome = [0, 1, 0, 0]
l1 = 0.0
l2 = 0.0
l3 = spatula_tip_to_tcp_dist
tip_hand_transform = [l1, l2, l3, 0, 0, 0]
vision_pos[2] = 0.17786528
#############
## Placing ##
#############
#if primitive called without placing arguments, go to center of bin
if rel_pose == None:
drop_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
drop_pose[3:7] = gripperOriHome
else:
drop_pose = ik.helper.drop_pose_transform(binId, rel_pose, BoxBody,
place_pose, viz_pub,
listener, br)
#~ Adjust height according to weigth
if is_drop:
drop_offset = 0.15
else:
drop_offset = 0.025
#~set drop pose target z position to be bottom of bin
drop_pose[2] = bin_pose[2] - rospy.get_param('tote/height') + drop_offset
#~Predrop: go to top middle bin surface fast without guarded move
predrop_pos = np.array(drop_pose[0:3])
predrop_pos[2] = bin_pose[2] + 0.05
######################
## Generate plans ##
######################
#~move safe to placing bin number
rospy.logdebug(
'[Picking] Collision free placing to binId %s and position %s', binId,
predrop_pos)
# if isExecute:
# q_initial = collision_free_placing(binId, listener, obj_ID=objId, BoxBody=BoxBody, isSuction = False,with_object = True, hand_orientation=drop_pose[3:7],projected_target_position = predrop_pos, isReturn = True)
# if update_command is not None:
# update_command.execute()
#~0.go up to avoid collision with tote walls
current_pose = ik.helper.get_tcp_pose(listener, tcp_offset=l3)
current_pose[2] = current_pose[2] + 0.15
plan, qf, plan_possible = generatePlan(q_initial,
current_pose[0:3],
current_pose[3:7],
tip_hand_transform,
'faster',
plan_name='go_up')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
#~0.1.fast motion to bin surface high
predrop_pos_high = predrop_pos
predrop_pos_high[2] = current_pose[2]
plan, qf, plan_possible = generatePlan(q_initial,
predrop_pos_high,
drop_pose[3:7],
tip_hand_transform,
'faster',
plan_name='go_bin_surface_high')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
#~1.fast motion to bin surface
predrop_pos[2] = bin_pose[2] + 0.05
plan, qf, plan_possible = generatePlan(q_initial,
predrop_pos,
drop_pose[3:7],
tip_hand_transform,
'faster',
plan_name='go_bin_surface')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
#~2. Guarded move down slow
plan, qf, plan_possible = generatePlan(q_initial,
drop_pose[0:3],
drop_pose[3:7],
tip_hand_transform,
'fast',
guard_on=WeightGuard(binId,
threshold=100),
backwards_speed='superSaiyan',
plan_name='guarded_drop')
if plan_possible:
plans_placing.append(plan)
q_initial = qf
else:
return compose_output()
#~3. open gripper
grasp_plan = EvalPlan('helper.moveGripper(0.110, 200)')
plans_placing2.append(grasp_plan)
#~4. open spatula
grasp_plan = EvalPlan('spatula.open()')
plans_placing2.append(grasp_plan)
#~5. go to predrop_pos
plan, qf, plan_possible = generatePlan(q_initial,
predrop_pos[0:3],
drop_pose[3:7],
tip_hand_transform,
'superSaiyan',
plan_name='predrop_pos')
if plan_possible:
plans_placing2.append(plan)
q_initial = qf
else:
return compose_output()
#~6. go to final pose
final_pos = bin_pose[0:3]
final_pos[2] = bin_pose[2] + 0.3
plan, qf, plan_possible = generatePlan(q_initial,
final_pos,
drop_pose[3:7],
tip_hand_transform,
'superSaiyan',
plan_name='final_pose')
if plan_possible:
plans_placing2.append(plan)
q_initial = qf
else:
return compose_output()
#~ Execute plans
if plan_possible:
if isExecute:
executePlanForward(plans, withPause)
#We start recording now
if recorder is not None:
lasers.start(binId)
recorder.start_recording(
action='placing',
action_id=str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")),
tote_num=binId,
frame_rate_ratio=5,
image_size=-1)
rospy.set_param('is_record', True)
executePlanForward(plans_placing, withPause)
if rospy.get_param('is_contact'):
ik.helper.move_cart(dz=0.015)
executePlanForward(plans_placing2, withPause)
rospy.set_param('is_record', False)
if recorder is not None:
recorder.stop_recording(save_action=True)
lasers.stop(binId)
execution_possible = True
return compose_output()
def sliderKpAtt(self):
position = self.horizontalSlider_kp_att.sliderPosition()
rospy.set_param('swarm_node/local_plan/kp_attractive',
(position / 50.0))
self.lineEdit_kp_att.setText(
str(rospy.get_param('swarm_node/local_plan/kp_attractive')))
def grasp(objInput,
listener,
br,
isExecute=True,
objId='expo_eraser',
binId=0,
withPause=False,
viz_pub=None,
recorder=None):
#########################################################
# fhogan and nikhilcd
# Description:
#~2017 picking primitive
#
#~Usage
#~ pick(objInput, listener, br, isExecute = True, objId = 'cheezit_big_original', flag = 1, withPause = False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None):
#
#~Parameters:
#
#~Inputs:
# 1) objInput: Input from vision or RViz (list of 12, or 7)
# 2) listener: Input from vision or RViz (list of 12, or 7)
# 3) br: Input from vision or RViz (list of 12, or 7)
# 4) isExecute: execute the robot motion if possible or not? (Bool)
# 5) objId: name of object (str)
# 6) binId: bin in which we need to place the objects. (It is useless when flag 0 and 1)
# 7) flag: Type of action directed by planning (0:run picking, 1:go to home (camera), 2: drop the object at target location)
# 8) withPause: Pause between robot motions (Bool)
# 9) rel_pose: object pose relative to link_6
# 10) BoxBody: Bounding box points resolved in link 6 frame
# 11) place_pose: desired position of bounding box
# 12) viz_pub: desired position of bounding box
#
#~Output:
# Dictionary of following keys:
# 1) grasp_possible: Does the object fit in the hand? (True/False)
# 2) plan_possible: Has the plan succeeded? (True/False)
# 3) execution_possible: Is there anything in the grasp? (True/False)
# 4) gripper_opening: gripper opening after grasp attempt (in meters)
# 5) graspPose: pose of the gripper at grasp (7X1 - position and quaternion)
# 6) gelsight_data: [] , dummy for now
#########################################################
#rospy.logdebug(msg, *args)
#rospy.loginfo(msg, *args)
#rospy.logwarn(msg, *args)
#rospy.logerr(msg, *args)
#rospy.logfatal(msg, *args)
###############################
## Pring input variables for ##
###############################
rospy.logdebug('[Picking] objInput %s', objInput)
rospy.logdebug('[Picking] objId %s', objId)
rospy.logdebug('[Picking] withPause %s', withPause)
########################
## Initialize values ##
########################
q_initial = ik.helper.get_joints()
bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
spatula_tip_to_tcp_dist = rospy.get_param(
"/gripper/spatula_tip_to_tcp_dist")
delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking')
vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3])
plans = []
plans_grasping = []
plans_grasping2 = []
plans_guarded = []
plans_guarded2 = []
plans_placing = []
plans_placing2 = []
graspPose = []
plan_possible = False
execution_possible = False
gripper_opening = 0.0
grasp_possible = True
gelsight_data = []
collision = False
final_object_pose = None
rospy.set_param('is_record', False)
rospy.set_param('is_contact', False)
def compose_output():
return {
'collision': collision,
'grasp_possible': grasp_possible,
'plan_possible': plan_possible,
'execution_possible': execution_possible,
'gripper_opening': gripper_opening,
'graspPose': graspPose,
'gelsight_data': gelsight_data,
'final_object_pose': final_object_pose
}
#########################
## Constant parameters ##
#########################
UpdistFromBinFast = 0.15 # Margin above object during "move to a location"
UpdistFromBinGuarded = 0.05 # Margin above object during "move to a location"
gripperOriHome = [0, 1, 0, 0]
l1 = 0.0
l2 = 0.0
l3 = spatula_tip_to_tcp_dist
tip_hand_transform = [l1, l2, l3, 0, 0, 0]
vision_pos[2] = 0.17786528
###############################
## Picking primitive ##
###############################
# ik.visualize_helper.visualize_grasping_proposals(viz_pub, np.asarray([objInput]), listener, br, False)
#~Define reference frames
world_X, world_Y, world_Z, tote_X, tote_Y, tote_Z, tote_pose_pos = ik.helper.reference_frames(
listener=listener, br=br)
#~get grasp pose and gripper opening from vision
if len(objInput) == 12:
graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_12(
objInput)
graspPos = graspPos # + hand_X*0.02*1
elif len(objInput) == 7:
graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_7(
objInput, objId, listener, br)
#frank hack for stationary spatula:
grasp_width = 0.11
#~build gripper orientation matrix 3x3
hand_orient_norm = np.vstack([hand_X, hand_Y, hand_Z])
hand_orient_norm = hand_orient_norm.transpose()
#~Grasp relocation script
hand_orient_quat = ik.helper.mat2quat(hand_orient_norm)
euler = tf.transformations.euler_from_quaternion(hand_orient_quat)
theta = euler[2] - np.pi
colFreePose = collisionFree(graspPos,
binId=binId,
listener=listener,
br=br,
finger_opening=grasp_width,
safety_margin=0.00,
theta=theta)
graspPos[0:2] = colFreePose[0:2]
#################################
## GENERATE PLANS OF PRIMITIVE ##
#################################.
#~ Start from home position
# if isExecute:
# q_initial = collision_free_placing(binId, listener, isSuction = False, with_object = False, hand_orientation=[0,1,0,0], projected_target_position = bin_pose[0:3], isReturn = True)
# scorpion.back()
#~0. Move to a location above the bin, Rotate gripper to grasp orientation
pregrasp_targetPosition = graspPos
pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinFast
plan, qf, plan_possible = generatePlan(q_initial,
pregrasp_targetPosition,
hand_orient_quat,
tip_hand_transform,
'superSaiyan',
plan_name='go_safe_bin')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
#~1. Move to to surface of bin
pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinGuarded
plan, qf, plan_possible = generatePlan(q_initial,
pregrasp_targetPosition,
hand_orient_quat,
tip_hand_transform,
'superSaiyan',
plan_name='go_top_bin')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
#~2. Open gripper
grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % grasp_width)
plans.append(grasp_plan)
#~ 3. Open spatula
grasp_plan = EvalPlan('spatula.open()')
plans.append(grasp_plan)
grasp_targetPosition = deepcopy(graspPos)
# rospy.loginfo('[Picking] Grasp Target %s', grasp_targetPosition)
#~4. sleep
sleep_plan = EvalPlan('rospy.sleep(0.2)')
plans.append(sleep_plan)
#~5. perform guarded move down
grasp_targetPosition[2] = bin_pose[2] - rospy.get_param(
'/tote/height') + 0.02 #~frank hack for safety
plan, qf, plan_possible = generatePlan(q_initial,
grasp_targetPosition,
hand_orient_quat,
tip_hand_transform,
'faster',
guard_on=WeightGuard(binId,
threshold=100),
plan_name='guarded_pick')
if plan_possible:
plans_guarded.append(plan)
q_initial = qf
else:
return compose_output()
#~7. Close spatula
grasp_plan = EvalPlan('spatula.close()')
plans_guarded2.append(grasp_plan)
#~8. Close gripper
grasp_plan = EvalPlan('helper.graspinGripper(%f,%f)' % (800, 30))
plans_guarded2.append(grasp_plan)
#~9. sleep
sleep_plan = EvalPlan('rospy.sleep(0.6)')
plans_guarded2.append(sleep_plan)
#~10. Move to a location above the bin
# plan, qf, plan_possible = generatePlan(q_initial, vision_pos, delta_vision_pose[3:7], tip_hand_transform, 'fastest', plan_name = 'retrieve_object')
# if plan_possible:
# plans_grasping.append(plan)
# q_initial = qf
# else:
# return compose_output()
################
## EXECUTION ###
################
if isExecute and plan_possible:
executePlanForward(plans, withPause)
###We start recording now
if recorder is not None:
lasers.start(binId)
recorder.start_recording(
action='grasping',
action_id=str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")),
tote_num=binId,
frame_rate_ratio=5,
image_size=-1)
#Execute guarded move
rospy.set_param('is_record', True)
executePlanForward(plans_guarded, withPause)
if rospy.get_param('is_contact'):
ik.helper.move_cart(dz=0.005)
executePlanForward(plans_guarded2, withPause)
rospy.set_param('is_record', False)
elif not isExecute:
execution_possible = plan_possible
return compose_output()
def sliderSafetyRange(self):
position = self.horizontalSlider_safety_range.sliderPosition()
rospy.set_param('swarm_node/local_plan/safety_range',
(position / 10.0))
self.lineEdit_safety_range.setText(
str(rospy.get_param('swarm_node/local_plan/safety_range')))
def sliderKdRep(self):
position = self.horizontalSlider_kd_rep.sliderPosition()
rospy.set_param('swarm_node/local_plan/kd_repulsive',
(position / 50.0))
self.lineEdit_kd_rep.setText(
str(rospy.get_param('swarm_node/local_plan/kd_repulsive')))
