def saveLocation(self, param_name):
rospy.set_param('location_dict', self.location_dict)
rosparam.dump_params('/home/athome/catkin_ws/src/mimi_common_pkg/config/'
+ param_name + '.yaml', '/location_dict')
print rosparam.get_param('location_dict')
rospy.loginfo('Saving complete')
return True
def __save(self):
file_name = os.path.expanduser(
os.path.join(
self.save_loc, 'flo_parameters-{}.yaml'.format(
datetime.now().strftime('%Y-%m-%d-%H-%M-%S'))))
rospy.loginfo('saving ros params to %s', file_name)
print(file_name)
os.mknod(file_name)
rosparam.dump_params(file_name, '/')
def save_params(pos, rot, filename):
rot_homo = np.eye(4)
rot_homo[:3, :3] = rot
rot_quat = tf_trans.quaternion_from_matrix(rot_homo)
optitrak_params = {"position": pos.T.A[0].tolist(), "orientation": rot_quat.tolist()}
print optitrak_params
rosparam.upload_params("optitrak_calibration", optitrak_params)
rospy.sleep(0.5)
rosparam.dump_params(filename, "optitrak_calibration")
def _save_map_request(self, req):
if req.directory:
map_path = req.directory + "/"
else:
map_path = os.path.expanduser(
'~') + '/.pal/' + self._robot + '_maps/'
map_name = 'config'
shutil.rmtree(map_path + map_name, ignore_errors=True)
os.makedirs(map_path + map_name)
rospy.logdebug("Saving map with map_name '" + map_name +
"' and map_path '" + map_path + "'")
# Save map (image and meta-data).
filename = 'submap_0'
pal_launch.execute_command('rosrun', 'map_server', 'map_saver', '-f',
filename)
# Update /mmap/numberOfSubMaps parameter
# [from saverFunctions::configSaver]
rospy.set_param('/mmap/numberOfSubMaps', 1)
map_data_file = os.path.join(map_path + map_name + '/', 'mmap.yaml')
if rospy.has_param('/mmap'):
rosparam.dump_params(map_data_file, '/mmap')
else:
try:
os.remove(map_data_file)
except OSError:
pass
map_data_file = None
# Move map to maps folder
# (this is required to have the image relative path in map.yaml):
shutil.move(filename + '.pgm', map_path + map_name)
shutil.move(filename + '.yaml', map_path + map_name + '/map.yaml')
# Create nice map
shutil.copyfile(os.path.join(map_path + map_name, 'submap_0.pgm'),
os.path.join(map_path + map_name, 'map.pgm'))
with open(os.path.join(map_path + map_name, 'transformation.xml'),
'w') as transf:
transf.write(DEFAULT_TRANSFORMATION)
# Set permisions
self._chmod(map_path, 0777)
# Save initial pose of the robot in the map
rospack = rospkg.RosPack()
shutil.copyfile(
rospack.get_path('pal_navigation_sm') + '/config/pose.yaml',
map_path + '../pose.yaml')
return SaveMapResponse(True, map_name, map_path,
'Map saved: %s' % map_name)
def save_params(self):
rospy.loginfo('Saving parameters')
if not os.path.exists(os.path.dirname(self.config_filepath)):
try:
os.makedirs(os.path.dirname(self.config_filepath))
except OSError as exc: # Guard against race condition
if exc.errno != errno.EEXIST:
raise
rosparam.dump_params(self.config_filepath, self.ns, True)
def save_file(self, filename):
## Data
data = {}
frames = {}
for frame in self.iter_frames(include_temp=False):
t = {}
t["x"] = float(frame.position[0])
t["y"] = float(frame.position[1])
t["z"] = float(frame.position[2])
o = {}
o["x"] = float(frame.orientation[0])
o["y"] = float(frame.orientation[1])
o["z"] = float(frame.orientation[2])
o["w"] = float(frame.orientation[3])
f = {}
f["parent"] = frame.parent
f["position"] = t
f["orientation"] = o
f["style"] = frame.style
if frame.style == "plane":
f["data"] = { "length": frame.length, "width":frame.width, "color": frame.color }
elif frame.style == "cube":
f["data"] = { "length": frame.length, "width": frame.width, "height": frame.height , "color": frame.color}
elif frame.style == "sphere":
f["data"] = { "diameter": frame.diameter, "color": frame.color }
elif frame.style == "axis":
f["data"] = { "length": frame.length, "width": frame.width, "color": frame.color }
elif frame.style == "mesh":
self.update_file_format(frame)
f["data"] = { "package" : frame.package, "path" : frame.path, "scale" : frame.scale, "color": frame.color }
frames[frame.name] = f
data["frames"] = frames
## To parameter server
rospy.set_param(self.namespace, data)
print rospy.get_param(self.namespace)
## Dump param to file
print "Saving to file", filename
rosparam.dump_params(filename, self.namespace)
print "Saving done"
return True
def execute(self, userdata):
try:
rospy.loginfo('Executing state: SAVING')
rospy.set_param('/location_dict', userdata.saving_in_data)
rosparam.dump_params(
'/home/athome/catkin_ws/src/mimi_common_pkg/config/location_dict.yaml',
'/location_dict')
rospy.loginfo('Saving complete')
return 'save_finish'
except rospy.ROSInterruptException:
rospy.loginfo('**Interrupted**')
pass
def save_params(pos, rot, filename):
rot_homo = np.eye(4)
rot_homo[:3, :3] = rot
rot_quat = tf_trans.quaternion_from_matrix(rot_homo)
optitrak_params = {
"position": pos.T.A[0].tolist(),
"orientation": rot_quat.tolist()
}
print optitrak_params
rosparam.upload_params("optitrak_calibration", optitrak_params)
rospy.sleep(0.5)
rosparam.dump_params(filename, "optitrak_calibration")
def save_params(p, filename):
ft_params = { "mass" : p[0],
"force_zero_x" : p[1],
"force_zero_y" : p[2],
"force_zero_z" : p[3],
"com_pos_x" : p[4],
"com_pos_y" : p[5],
"com_pos_z" : p[6],
"torque_zero_x" : p[7],
"torque_zero_y" : p[8],
"torque_zero_z" : p[9] }
rosparam.upload_params(rospy.get_name(), ft_params)
rospy.sleep(0.5)
rosparam.dump_params(filename, rospy.get_name())
def _save_map_request(self, req):
if req.directory:
map_path = req.directory + "/"
else:
map_path = os.path.expanduser('~') + '/.pal/' + self._robot + '_maps/'
map_name = 'config'
shutil.rmtree(map_path+map_name, ignore_errors=True)
os.makedirs(map_path+map_name)
rospy.logdebug("Saving map with map_name '" + map_name + "' and map_path '" + map_path + "'")
# Save map (image and meta-data).
filename = 'submap_0'
pal_launch.execute_command('rosrun', 'map_server', 'map_saver', '-f', filename)
# Update /mmap/numberOfSubMaps parameter
# [from saverFunctions::configSaver]
rospy.set_param('/mmap/numberOfSubMaps', 1)
map_data_file = os.path.join(map_path+map_name+'/', 'mmap.yaml')
if rospy.has_param('/mmap'):
rosparam.dump_params(map_data_file, '/mmap')
else:
try:
os.remove(map_data_file)
except OSError:
pass
map_data_file = None
# Move map to maps folder
# (this is required to have the image relative path in map.yaml):
shutil.move(filename + '.pgm', map_path+map_name)
shutil.move(filename + '.yaml',map_path+map_name+'/map.yaml')
# Create nice map
shutil.copyfile(os.path.join(map_path+map_name, 'submap_0.pgm'), os.path.join(map_path+map_name, 'map.pgm'))
with open(os.path.join(map_path+map_name, 'transformation.xml'), 'w') as transf:
transf.write(DEFAULT_TRANSFORMATION)
# Set permisions
self._chmod(map_path, 0777)
# Save initial pose of the robot in the map
rospack = rospkg.RosPack()
shutil.copyfile(rospack.get_path('pal_navigation_sm') + '/config/pose.yaml', map_path+'../pose.yaml')
return SaveMapResponse(True, map_name, map_path, 'Map saved: %s' % map_name)
def save_params(p, filename):
ft_params = {
"mass": p[0],
"force_zero_x": p[1],
"force_zero_y": p[2],
"force_zero_z": p[3],
"com_pos_x": p[4],
"com_pos_y": p[5],
"com_pos_z": p[6],
"torque_zero_x": p[7],
"torque_zero_y": p[8],
"torque_zero_z": p[9]
}
rosparam.upload_params(rospy.get_name(), ft_params)
rospy.sleep(0.5)
rosparam.dump_params(filename, rospy.get_name())
def dump(self):
rosparam.dump_params("test.yaml", "artag")
print "yaml file saved"
for marker in actual_markers)
if camera.set_k_points(markers_centers):
parametrs = camera.get_parametrs_dict()
parametrs_dict[str(camera.id)] = parametrs
if SAVED:
cv2.putText(img, "SAVED!", (20, 30), cv2.FONT_HERSHEY_COMPLEX,
0.8, (0, 255, 100), 3)
cv2.imshow(str(camera.id), img)
if cv2.waitKey(10) & 0xFF == 27:
RUN = not RUN
if len(parametrs_dict.keys()) == len(cameras):
if not SAVED:
cam_params = {'cam_params': parametrs_dict}
fname = "../config/cameras_k_points.config"
rosparam.set_param_raw("cameras_k_points",
cam_params,
verbose=False)
rosparam.dump_params(fname, "cameras_k_points", verbose=False)
SAVED = True
for camera in cameras:
camera.stop_stream()
cv2.destroyAllWindows()
rospy.is_shutdown()
rospy.signal_shutdown(shutdown)
def dump():
rosparam.dump_params("smach.yaml", "smach")
LICENSE file in the root directory of this source tree.
@author: Jan Behrens
"""
import rospy
import rosparam
if __name__ == "__main__":
param_ns = 'SolverSetup'
rospy.set_param('SolverSetup/ROBOT_NAME', "kuka")
rospy.set_param('SolverSetup/STEPS_MAX', 50)
rospy.set_param('SolverSetup/TIME_MAX', 10000)
rospy.set_param('SolverSetup/LUBY_RESTART_CONSTANT', 5)
rospy.set_param('SolverSetup/BASE_SEED', 5)
rospy.set_param('SolverSetup/SEED',
rospy.get_param('~setup/BASE_SEED', default=5))
rospy.set_param('SolverSetup/solver_time', 30000)
rospy.set_param(
param_ns + '/ROADMAP_NAMES', {
'r2_arm': 'prm_r2_arm_2018-05-30 15:09:33.898024',
'r1_arm': 'prm_r1_arm_2018-05-30 14:57:07.705629'
})
rospy.set_param(param_ns + '/CLASH_NAME', "clash_1528066416")
rospy.set_param(param_ns + '/GROUP_NAMES', ['r2_arm', 'r1_arm'])
rospy.set_param(param_ns + '/COLLISION', True)
rospy.set_param(param_ns + '/EXECUTE_MOTION', True)
rosparam.dump_params("planner_params.yaml", "SolverSetup/", True)
class calibrate_camera:
def __init__(self):
try:
self.no_frames = rospy.get_param('/camera_calibration/frames')
except:
self.no_frames = 1000
print self.no_frames
self.pattern_size = (6, 8)
self.pattern_points = np.zeros((np.prod(self.pattern_size), 3),
np.float32)
self.pattern_points[:, :2] = np.indices(self.pattern_size).T.reshape(
-1, 2)
self.pattern_points[:, 0] = -self.pattern_points[:, 0]
self.pattern_points *= 0.05
self.count = [1, 1]
self.left_midXYZ = [0, 0, 0]
self.left_midQ = [0, 0, 0, 0]
self.right_midXYZ = [0, 0, 0]
self.right_midQ = [0, 0, 0, 0]
self.final_left_xyz = [0, 0, 0]
self.final_left_q = [0, 0, 0, 1]
self.final_right_xyz = [0, 0, 0]
self.final_right_q = [0, 0, 0, 1]
self.still_calibrating = True
self.bridge = CvBridge()
self.left_image = rospy.Subscriber('/left/rgb/image_color', Image,
self.left_callback)
self.right_image = rospy.Subscriber('/right/rgb/image_color', Image,
self.right_callback)
self.br = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.pt = {
'previous_transform': {
'left': {
'orientation': [0.0, 0.0, 0.0, 0.0],
'position': [0.0, 0.0, 0.0]
},
'right': {
'orientation': [0.0, 0.0, 0.0, 0.0],
'position': [0.0, 0.0, 0.0]
}
}
}
self.rospack = rospkg.RosPack()
try:
self.pt = rosparam.load_file(
self.rospack.get_path('camera_setup') +
'/config/previous_transform.yaml')[0][0]
except:
self.still_calibrating = True
if rospy.get_param('~quick'):
self.final_left_xyz = self.pt['previous_transform']['left'][
'position']
self.final_left_q = self.pt['previous_transform']['left'][
'orientation']
self.final_right_xyz = self.pt['previous_transform']['right'][
'position']
self.final_right_q = self.pt['previous_transform']['right'][
'orientation']
self.still_calibrating = False
self.publish_feedback()
def find_tf(self, ros_image_msg, invert):
try:
#Colour Image
cv_image = self.bridge.imgmsg_to_cv2(ros_image_msg, "bgr8")
except CvBridgeError, e:
print e
cv_image = cv2.cvtColor(cv2.cvtColor(cv_image, cv2.COLOR_RGB2GRAY),
cv2.COLOR_GRAY2RGB)
found, corners = cv2.findCirclesGrid(cv_image,
self.pattern_size,
flags=cv2.CALIB_CB_SYMMETRIC_GRID)
obj_points = []
img_points = []
if found:
img_points.append(corners.reshape(-1, 2))
if invert:
img_points = np.flipud(np.fliplr(img_points))
obj_points.append(self.pattern_points)
#print obj_points
camera_matrix = np.array(
[525.0, 0.0, 319.5, 0.0, 525.0, 239.5, 0.0, 0.0,
1.0]).reshape(3, 3)
dist_coefs = np.empty((5, 1))
_, rVec, tVec = cv2.solvePnP(np.array(obj_points),
np.array(img_points), camera_matrix,
dist_coefs)
Rt = cv2.Rodrigues(rVec)[0]
rot_hom = np.eye(4)
rot_hom[:3, :3] = Rt
q = tf.transformations.quaternion_from_matrix(rot_hom)
if invert:
self.right_midXYZ = np.vstack(
(self.right_midXYZ, tVec.reshape(1, -1)[0]))
self.right_midQ = np.vstack((self.right_midQ, q))
self.count[1] += 1
self.update_progress()
if self.count[1] == self.no_frames:
self.final_right_xyz = np.median(self.right_midXYZ[1:],
axis=0)
median_Q = np.median(self.right_midQ[1:], axis=0)
self.final_right_q = median_Q / np.sum(median_Q)
else:
self.left_midXYZ = np.vstack(
(self.left_midXYZ, tVec.reshape(1, -1)[0]))
self.left_midQ = np.vstack((self.left_midQ, q))
self.count[0] += 1
#print self.count
self.update_progress()
if self.count[0] == self.no_frames:
self.final_left_xyz = np.median(self.left_midXYZ[1:],
axis=0)
median_Q = np.median(self.left_midQ[1:], axis=0)
self.final_left_q = median_Q / np.sum(median_Q)
if self.count[0] > self.no_frames and self.count[
1] > self.no_frames and self.still_calibrating:
self.still_calibrating = False
print "Successfully Calibrated over %d frames, " % self.no_frames
self.pt['previous_transform']['left'][
'position'] = self.final_left_xyz.tolist()
self.pt['previous_transform']['right'][
'position'] = self.final_right_xyz.tolist()
self.pt['previous_transform']['left'][
'orientation'] = self.final_left_q.tolist()
self.pt['previous_transform']['right'][
'orientation'] = self.final_right_q.tolist()
print self.pt
rospy.set_param('calibrate_camera', self.pt)
rosparam.dump_params(self.rospack.get_path('camera_setup') +
'/config/previous_transform.yaml',
'calibrate_camera',
verbose=False)
self.publish_feedback()
def dump(self):
rosparam.dump_params("location.yaml", "artag")
