def __init__(self, model_to_track_list):
# This are the models that we will generate information about.
self.gz_model_obj = GazeboModel(model_to_track_list)
self.people = People()
self.person = Person()
self.pub = rospy.Publisher("/people", People, queue_size=10)
self.pub2 = rospy.Publisher("/person_pose", People, queue_size=10)
def __init__(self, environment_name, weight_file_name, image_size, ALPHA,
grasp_activated, number_of_elements_to_be_output):
self._image_size = image_size
self._number_of_elements_to_be_output = number_of_elements_to_be_output
self.init_rviz_markers()
# Init service to move fetch, this is because moveit doenst work in python 3
self._grasp_activated = grasp_activated
if self._grasp_activated == True:
self.fetch_move_client = MoveFetchClient()
self.fetch_move_client.go_to_safe_arm_pose()
# Init camera RGB object
self.rgb_camera_object = RGBCamera("/dynamic_objects/camera/raw_image")
# This are the models that we will generate information about.
self.model_to_track_name = "demo_spam1"
self.table_to_track_name = "demo_table1"
model_to_track_list = [
self.model_to_track_name, self.table_to_track_name
]
self.gz_model_obj = GazeboModel(model_to_track_list)
# We start the model in Keras
self.model = create_model(self._image_size, ALPHA,
self._number_of_elements_to_be_output)
rospack = rospkg.RosPack()
# get the file path for rospy_tutorials
path_to_package = rospack.get_path('my_dcnn_training_pkg')
models_weight_checkpoints_folder = os.path.join(path_to_package, "bk")
model_file_path = os.path.join(models_weight_checkpoints_folder,
weight_file_name)
print(model_file_path)
self.model.load_weights(model_file_path)
self.testing_unscaled_img_folder = os.path.join(
path_to_package, "testing/dataset_gen/images")
self.testing_unscaled_anotations_folder = os.path.join(
path_to_package, "testing/dataset_gen_annotations")
# We reset the environent to a random state
print("Starting Service to Reset World Randomly....")
self.dynamic_world_service_call = rospy.ServiceProxy(
'/dynamic_world_service', Empty)
self.change_env_request = EmptyRequest()
self.dynamic_world_service_call(self.change_env_request)
print("Starting Service to Reset World Randomly....DONE")
class ModelPoseStore():
def __init__(self, model_to_track_list):
# This are the models that we will generate information about.
self.gz_model_obj = GazeboModel(model_to_track_list)
def get_pose_of_model(self, model_name):
"""
Retrieves the position of an object from the world
"""
pose_now = self.gz_model_obj.get_model_pose(model_name)
return pose_now
def store_model_poses_for_duration(self,
model_name,
duration=1.0,
frequency_save_pose=1.0,
file_to_store="poses.yaml"):
"""
We store in the Given File Name the poses of the given model, whihc has to be inside the init model list
inside a Yaml file
"""
rate = rospy.Rate(frequency_save_pose)
init_time_stamp = rospy.get_time()
now_time_stamp = rospy.get_time()
delta_time = now_time_stamp - init_time_stamp
with open(file_to_store, 'w') as outfile:
while delta_time <= duration:
now_pose = self.get_pose_of_model(model_name)
now_dict = self.reformat_pose_to_dict(now_pose)
rospy.logdebug(str(now_dict))
yaml.dump(now_dict, outfile, default_flow_style=False)
rate.sleep()
now_time_stamp = rospy.get_time()
delta_time = now_time_stamp - init_time_stamp
rospy.logdebug("TIME SAVING==>" + str(delta_time))
def reformat_pose_to_dict(self, pose):
"""
Converts Pose to dict
"""
dictionary = message_converter.convert_ros_message_to_dictionary(pose)
now_time_stamp = rospy.get_time()
stamp_dict = {str(now_time_stamp): dictionary}
return stamp_dict
class ModelPoseStore():
def __init__(self, model_to_track_list):
# This are the models that we will generate information about.
self.gz_model_obj = GazeboModel(model_to_track_list)
self.people = People()
self.person = Person()
self.pub = rospy.Publisher("/people", People, queue_size=10)
self.pub2 = rospy.Publisher("/person_pose", People, queue_size=10)
def get_pose_of_model(self, model_name):
"""
Retrieves the position of an object from the world
"""
pose_now = self.gz_model_obj.get_model_pose(model_name)
return pose_now
def store_model_poses_for_duration(self, model_name, duration=1.0, frequency_save_pose=1.0, file_to_store="poses.yaml"):
"""
We store in the Given File Name the poses of the given model, whihc has to be inside the init model list
inside a Yaml file
"""
rate = rospy.Rate(10)
while True:
now_pose = self.get_pose_of_model(model_name)
self.person.position.x = now_pose.position.x
self.person.position.y = now_pose.position.y
self.people.header.frame_id = "robot_map"
self.people.people.append(self.person)
rospy.logdebug(str(self.people.people[0]))
#rospy.logdebug(str(self.person))
self.pub.publish(self.people)
self.pub2.publish(self.people)
self.people.people.pop()
rate.sleep()
def reformat_pose_to_dict(self, pose):
"""
Converts Pose to dict
"""
dictionary = message_converter.convert_ros_message_to_dictionary(pose)
now_time_stamp = rospy.get_time()
stamp_dict = {str(now_time_stamp): dictionary}
return stamp_dict
def __init__(self, model_name, init_coordinates):
self.found_coordinates = False
self._model_name = model_name
self._model_coordinates = init_coordinates
self.current_cmd_speed = Twist()
self.g_pause = rospy.ServiceProxy("/gazebo/pause_physics", EmptySrv)
self.g_unpause = rospy.ServiceProxy("/gazebo/unpause_physics",
EmptySrv)
self.g_set_state = rospy.ServiceProxy("/gazebo/set_model_state",
SetModelState)
rospy.Subscriber(model_name + "/cmd_vel", Twist, self.move_callback)
self.gz_model = GazeboModel(robots_name_list=[model_name])
self._trajectory = Trajectory(num_points=100,
type="circle",
radius=3.0,
height=1.0)
def publisher_of_tf():
rospy.init_node('publisher_of_tf_node', anonymous=True)
robot_name_list = ["dahei"]
gazebo_model_object = GazeboModel(robot_name_list)
for robot_name in robot_name_list:
pose_now = gazebo_model_object.get_model_pose(robot_name)
# Leave time enough to be sure the Gazebo Model data is initalised
time.sleep(1)
rospy.loginfo("Ready..Starting to record now...")
rate = rospy.Rate(5) # 5hz
while not rospy.is_shutdown():
for robot_name in robot_name_list:
pose_now = gazebo_model_object.get_model_pose(robot_name)
if not pose_now:
print("The Pose is not yet"+str(robot_name)+" available...Please try again later")
else:
x.append(round(pose_now.position.x,3))
y.append(round(pose_now.position.y,3))
print("recording")
rate.sleep()
def __init__(self, model_to_track_list):
# This are the models that we will generate information about.
self.gz_model_obj = GazeboModel(model_to_track_list)
def dataset_generator(models_of_interest_list,
models_to_be_list,
full_path_to_dataset_gen='./dataset_gen/',
number_of_dataset_elements=10,
frequency=1,
show_images=False,
env_change_settle_time=2.0,
move_arm=False,
move_time=2.0,
index_img=0):
# We first wait for the service for RandomEnvironment change to be ready
rospy.loginfo("Waiting for service /dynamic_world_service to be ready...")
rospy.wait_for_service('/dynamic_world_service')
rospy.loginfo("Service /dynamic_world_service READY")
dynamic_world_service_call = rospy.ServiceProxy('/dynamic_world_service',
Empty)
change_env_request = EmptyRequest()
dynamic_world_service_call(change_env_request)
# Init to move the robot arm
move_arm_client_object = MoveArmClient()
x_range = [0.35, 0.35]
y_range = [0.1, 0.5]
z_range = [0.64 + 0.3, 0.64 + 0.3]
orientation_XYZW = [-0.707, 0.000, 0.707, 0.001]
#Init Pose
move_arm_client_object.go_to_safe_arm_pose()
# Period show the images in miliseconds
period = int((1 / frequency) * 1000)
# We first check if the path given exists, if not its generated the directory
if os.path.exists(full_path_to_dataset_gen):
shutil.rmtree(full_path_to_dataset_gen)
os.makedirs(full_path_to_dataset_gen)
rospy.logfatal("Created folder=" + str(full_path_to_dataset_gen))
# Init camera RGB object
camera_topic = "/robot1_camera/rgb/image_raw"
camera_info_topic = "/robot1_camera/rgb/camera_info"
camera_obj = RGBCamera(camera_topic, camera_info_topic)
# This are the models that we will generate information about.
gz_model_obj = GazeboModel(models_to_be_list)
# XML generator :
xml_generator_obj = XMLGenerator(path_out=full_path_to_dataset_gen)
# index to name the pictures :
now = datetime.datetime.now()
str_date = now.strftime("%Y_%m_%d_%H_%M_")
# create the images folder :
path_img_dir = os.path.join(full_path_to_dataset_gen, 'images/')
if not os.path.exists(path_img_dir):
os.makedirs(path_img_dir)
# We generate as many dataset elements as it was indicated.
for i in range(number_of_dataset_elements):
# We Randomise Environment
dynamic_world_service_call(change_env_request)
rospy.logwarn("Waiting for new env to settle...")
rospy.sleep(env_change_settle_time)
rospy.logwarn("Waiting for new env to settle")
# We move Arm to random pose
if move_arm:
rospy.logwarn("Moving to Random Pose TCP")
move_arm_client_object.move_tcp_to_random_pose(
x_range, y_range, z_range, orientation_XYZW)
rospy.sleep(move_time)
rospy.logwarn("Moving to Random Pose TCP DONE")
else:
move_time = 0.0
estimated_time_of_completion = ((number_of_dataset_elements - i) *
(env_change_settle_time +
(period / 1000.0) + move_time)) / 60.0
rospy.logwarn("Time Estimated of completion [min]==>" +
str(estimated_time_of_completion))
# We take picture
rospy.logerr("Takeing PICTURE")
#image = camera_obj.getImage()
image = camera_obj.get_latest_image()
rospy.logerr("Takeing PICTURE...DONE")
#cam_info = camera_obj.get_camera_info()
cam_info = camera_obj.get_camera_info()
if image is None:
rospy.logwarn("Image value was none")
image = numpy.zeros((cam_info.height, cam_info.width))
filename = str_date + str(index_img)
# We retrieve objects position
pose_models_oi_dict = {}
for model_name in models_of_interest_list:
rospy.logdebug("model_name==>" + str(model_name))
pose_now = gz_model_obj.get_model_pose(model_name)
pose_models_oi_dict[model_name] = pose_now
rospy.logdebug("ObjectsOfInterest poses==>" + str(pose_models_oi_dict))
#raw_input("Press After Validate Position in simulation....")
# We create the XML list tags for each Model of Interest captured before
listobj = []
for model_name, model_3dpose in pose_models_oi_dict.items():
x_com = model_3dpose.position.x
y_com = model_3dpose.position.y
z_com = model_3dpose.position.z
quat_x = model_3dpose.orientation.x
quat_y = model_3dpose.orientation.y
quat_z = model_3dpose.orientation.z
quat_w = model_3dpose.orientation.w
pose3d = [x_com, y_com, z_com, quat_x, quat_y, quat_z, quat_w]
listobj.append(XMLObjectTags(name=model_name, pose3d=pose3d))
image_format_extension = ".png"
xml_generator_obj.generate(object_tags=listobj,
filename=filename,
extension_file=image_format_extension,
camera_width=cam_info.width,
camera_height=cam_info.height)
index_img += 1
file_name_ext = filename + image_format_extension
path_img_file = os.path.join(path_img_dir, file_name_ext)
cv2.imwrite(path_img_file, image)
if show_images:
cv2.imshow(filename, image)
cv2.waitKey(period)
cv2.destroyAllWindows()
class MoveModel(object):
def __init__(self, model_name, init_coordinates):
self.found_coordinates = False
self._model_name = model_name
self._model_coordinates = init_coordinates
self.current_cmd_speed = Twist()
self.g_pause = rospy.ServiceProxy("/gazebo/pause_physics", EmptySrv)
self.g_unpause = rospy.ServiceProxy("/gazebo/unpause_physics",
EmptySrv)
self.g_set_state = rospy.ServiceProxy("/gazebo/set_model_state",
SetModelState)
rospy.Subscriber(model_name + "/cmd_vel", Twist, self.move_callback)
self.gz_model = GazeboModel(robots_name_list=[model_name])
self._trajectory = Trajectory(num_points=100,
type="circle",
radius=3.0,
height=1.0)
def move_callback(self, msg):
self.current_cmd_speed = msg
def move_model(self, coordinates_to_move_to, pause_physics=False):
if pause_physics:
rospy.loginfo("Start Pausing Physics")
rospy.wait_for_service("/gazebo/pause_physics")
rospy.loginfo("Pausing physics")
try:
self.g_pause()
except Exception as e:
rospy.logerr('Error on calling service: %s', str(e))
#rospy.loginfo("Filling Pose Data")
pose = Pose()
pose.position.x = coordinates_to_move_to.x
pose.position.y = coordinates_to_move_to.y
pose.position.z = coordinates_to_move_to.z
quaternion = tf.transformations.quaternion_from_euler(
coordinates_to_move_to.roll, coordinates_to_move_to.pitch,
coordinates_to_move_to.yaw)
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
# We set twist to Null to remove any prior movements
twist = Twist()
linear = Vector3()
angular = Vector3()
linear.x = 0.0
linear.y = 0.0
linear.z = 0.0
angular.x = 0.0
angular.y = 0.0
angular.z = 0.0
twist.linear = linear
twist.angular = angular
state = ModelState()
state.model_name = self._model_name
state.pose = pose
state.twist = twist
#print state
#rospy.loginfo("Moving model = " + str(self._model_name))
try:
ret = self.g_set_state(state)
#print ret.status_message
except Exception as e:
rospy.logerr('Error on calling service: %s', str(e))
if pause_physics:
rospy.loginfo("Unpausing physics")
try:
self.g_unpause()
except Exception as e:
rospy.logerr('Error on calling service: %s', str(e))
def move_step_trajectory(self):
coordinates_to_move_to = self._trajectory.step_trajectory(loop=True)
self.move_model(coordinates_to_move_to)
def calculate_coord_for_speed(self, publish_rate, ball=False):
"""
Gets the current position of the model and adds the increment based on the Publish rate
"""
pose_now = self.gz_model.get_model_pose(robot_name=self._model_name)
if pose_now:
# space_increment = speed * Frequency
z_increment = self.current_cmd_speed.angular.z * (1.0 /
publish_rate)
explicit_quat = [
pose_now.orientation.x, pose_now.orientation.y,
pose_now.orientation.z, pose_now.orientation.w
]
pose_now_euler = tf.transformations.euler_from_quaternion(
explicit_quat)
if ball:
roll = 0.0
pitch = 0.0
yaw = 1.6
planar_increment = self.current_cmd_speed.linear.x * \
(1.0/publish_rate)
x_yaw_comp = math.cos(yaw)
y_yaw_comp = math.sin(yaw)
elevation_increment = self.current_cmd_speed.linear.z * \
(1.0/publish_rate)
#quaternion = tf.transformations.quaternion_from_euler(roll,pitch,yaw)
coordinates_to_move_to = Coordinates(
x=(pose_now.position.x + planar_increment),
y=(pose_now.position.y + z_increment),
z=(pose_now.position.z + elevation_increment),
roll=roll,
pitch=pitch,
yaw=yaw)
else:
roll = pose_now_euler[0]
pitch = pose_now_euler[1]
yaw = pose_now_euler[2] + z_increment
planar_increment = self.current_cmd_speed.linear.x * \
(1.0/publish_rate)
x_yaw_comp = math.cos(yaw)
y_yaw_comp = math.sin(yaw)
elevation_increment = self.current_cmd_speed.linear.z * \
(1.0/publish_rate)
#quaternion = tf.transformations.quaternion_from_euler(roll,pitch,yaw)
coordinates_to_move_to = Coordinates(
x=(pose_now.position.x + planar_increment * x_yaw_comp),
y=(pose_now.position.y + planar_increment * y_yaw_comp),
z=(pose_now.position.z + elevation_increment),
roll=roll,
pitch=pitch,
yaw=yaw)
else:
coordinates_to_move_to = None
return coordinates_to_move_to
def move_step_speed(self, publish_rate):
coordinates_to_move_to = self.calculate_coord_for_speed(publish_rate)
if coordinates_to_move_to:
self.move_model(coordinates_to_move_to)
if not self.found_coordinates:
rospy.loginfo("Coordinates Available...")
self.found_coordinates = True
else:
rospy.logwarn("No Coordinates available yet...")
def move_ball_step_speed(self, publish_rate):
"""
We move based on the cmd vel making the object move without orientation change
"""
coordinates_to_move_to = self.calculate_coord_for_speed(
publish_rate, True)
if coordinates_to_move_to:
self.move_model(coordinates_to_move_to)
if not self.found_coordinates:
rospy.loginfo("Coordinates Available...")
self.found_coordinates = True
else:
rospy.logwarn("No Coordinates available yet...")
class FetchDCNN():
def __init__(self, environment_name, weight_file_name, image_size, ALPHA,
grasp_activated, number_of_elements_to_be_output):
self._image_size = image_size
self._number_of_elements_to_be_output = number_of_elements_to_be_output
self.init_rviz_markers()
# Init service to move fetch, this is because moveit doenst work in python 3
self._grasp_activated = grasp_activated
if self._grasp_activated == True:
self.fetch_move_client = MoveFetchClient()
self.fetch_move_client.go_to_safe_arm_pose()
# Init camera RGB object
self.rgb_camera_object = RGBCamera("/dynamic_objects/camera/raw_image")
# This are the models that we will generate information about.
self.model_to_track_name = "demo_spam1"
self.table_to_track_name = "demo_table1"
model_to_track_list = [
self.model_to_track_name, self.table_to_track_name
]
self.gz_model_obj = GazeboModel(model_to_track_list)
# We start the model in Keras
self.model = create_model(self._image_size, ALPHA,
self._number_of_elements_to_be_output)
rospack = rospkg.RosPack()
# get the file path for rospy_tutorials
path_to_package = rospack.get_path('my_dcnn_training_pkg')
models_weight_checkpoints_folder = os.path.join(path_to_package, "bk")
model_file_path = os.path.join(models_weight_checkpoints_folder,
weight_file_name)
print(model_file_path)
self.model.load_weights(model_file_path)
self.testing_unscaled_img_folder = os.path.join(
path_to_package, "testing/dataset_gen/images")
self.testing_unscaled_anotations_folder = os.path.join(
path_to_package, "testing/dataset_gen_annotations")
# We reset the environent to a random state
print("Starting Service to Reset World Randomly....")
self.dynamic_world_service_call = rospy.ServiceProxy(
'/dynamic_world_service', Empty)
self.change_env_request = EmptyRequest()
self.dynamic_world_service_call(self.change_env_request)
print("Starting Service to Reset World Randomly....DONE")
def init_rviz_markers(self):
"""
We initialise the markers used to visualise the predictions vs the
real data.
"""
# We initialise the Markers for Center Of Mass estimation and the Object real position
marker_type = "cube"
namespace = "demo_table"
mesh_package_path = ""
self.demo_table_position = MarkerBasics(
type=marker_type,
namespace=namespace,
index=0,
red=0.0,
green=0.0,
blue=1.0,
alfa=1.0,
scale=[0.6, 0.6, 0.6],
mesh_package_path=mesh_package_path)
marker_type = "mesh"
namespace = "spam_real_position"
mesh_package_path = "dynamic_objects/models/demo_spam/meshes/nontextured_fixed.stl"
self.spam_real_position = MarkerBasics(
type=marker_type,
namespace=namespace,
index=0,
red=1.0,
green=0.0,
blue=0.0,
alfa=1.0,
scale=[1.0, 1.0, 1.0],
mesh_package_path=mesh_package_path)
marker_type = "sphere"
mesh_package_path = ""
namespace = "com_prediction"
self.com_prediction_marker = MarkerBasics(
type=marker_type,
namespace=namespace,
index=0,
red=0.0,
green=1.0,
blue=0.0,
alfa=1.0,
scale=[0.1, 0.1, 0.1],
mesh_package_path=mesh_package_path)
#self.pick_object_menu = PickObjectMenu()
def publish_markers_new_data(self, pred, reality, reality_table):
spam_real_pose = Pose()
spam_real_pose.position.x = reality[0]
spam_real_pose.position.y = reality[1]
spam_real_pose.position.z = reality[2]
com_prediction_pose = Pose()
com_prediction_pose.position.x = pred[0]
com_prediction_pose.position.y = pred[1]
com_prediction_pose.position.z = pred[2]
demo_table_pose = Pose()
demo_table_pose.position.x = reality_table[0]
demo_table_pose.position.y = reality_table[1]
demo_table_pose.position.z = reality_table[2]
self.spam_real_position.publish_marker(spam_real_pose)
self.com_prediction_marker.publish_marker(com_prediction_pose)
self.demo_table_position.publish_marker(demo_table_pose)
"""
def publish_new_grasp_state(self, grasp_state):
if (grasp_state == "grasp_success"):
print(">>>>grasp_success....")
self.pick_object_menu.update_menu(index=1)
elif (grasp_state == "grasp_fail"):
print(">>>>grasp_fail....")
self.pick_object_menu.update_menu(index=2)
else:
print(">>>>nothing....")
self.pick_object_menu.update_menu(index=0)
"""
def predict_image(self, model, cv2_img=None, path=None):
if cv2_img.all() != None:
im = cv2_img
else:
if path != None:
im = cv2.imread(path)
else:
print("Predict Image had no path image or image CV2")
return None
if im.shape[0] != self._image_size:
im = cv2.resize(im, (self._image_size, self._image_size))
"""
self.rgb_camera_object.display_image( image_display=im,
life_time_ms=50,
name="ResizedCAM"
)
"""
image = np.array(im, dtype='f')
image = preprocess_input(image)
self.rgb_camera_object.display_image(image_display=image,
life_time_ms=50,
name="ImagePredict")
prediction = model.predict(x=np.array([image]))[0]
return prediction
def show_image(self, path, wait_time=500):
image = cv2.imread(path)
cv2.imshow("image", image)
print("Waiting " + str(wait_time) + "ms...")
cv2.waitKey(wait_time)
print("Waiting " + str(wait_time) + "ms...END")
cv2.destroyAllWindows()
def get_xyz_from_xml(self, path_xml_file):
tree = ET.parse(path_xml_file)
x_com = float(tree.findtext("./object/pose3d/x_com"))
y_com = float(tree.findtext("./object/pose3d/y_com"))
z_com = float(tree.findtext("./object/pose3d/z_com"))
return [x_com, y_com, z_com]
def get_xyz_from_world(self, model_name):
"""
Retrieves the position of an object from the world
"""
pose_now = self.gz_model_obj.get_model_pose(model_name)
XYZ = [pose_now.position.x, pose_now.position.y, pose_now.position.z]
return XYZ
def start_prediction_test(self):
print("\nTrying out unscaled image")
for k in os.listdir(self.testing_unscaled_img_folder):
print("Name File==>" + str(k))
rospy.logwarn("We Reset Simulation")
init_joints_config = [0.0] * 7
self.move_joints(init_joints_config)
if "png" in k:
img_path = os.path.join(self.testing_unscaled_img_folder, k)
pred = self.predict_image(img_path, self.model)
base_name_file = os.path.splitext(k)[0]
annotation_file = base_name_file + ".xml"
img_anotation_path = os.path.join(
self.testing_unscaled_anotations_folder, annotation_file)
reality = self.get_xyz_from_xml(img_anotation_path)
print("Class Prediction=>" + str(pred))
print("Class Reality File Image=>" + str(reality))
self.publish_markers_new_data(pred, reality)
self.show_image(img_path, wait_time=5000)
if self._grasp_activated == True:
self.start_grasp_sequence(pred)
rospy.loginfo("Start Prediction DONE...")
def start_camera_rgb_prediction(self,
number_of_tests=1,
camera_period=5.0,
wait_reset_period=3.0,
go_dustbin=False):
for i in range(number_of_tests):
print("Number of Image=>" + str(i))
self.dynamic_world_service_call(self.change_env_request)
print("Waiting for Reset Env to settle=>")
rospy.sleep(wait_reset_period)
print("Waiting for Reset Env to settle...DONE")
cv2_img = self.rgb_camera_object.get_latest_image()
pred = self.predict_image(self.model, cv2_img)
# Add Z Axis value
z_value = 0.6 + 0.042
pred_mod = [pred[0], pred[1], z_value]
reality = self.get_xyz_from_world(self.model_to_track_name)
reality_table = self.get_xyz_from_world(self.table_to_track_name)
print("Class Prediction=>" + str(pred))
print("Class Prediction Corrected=>" + str(pred_mod))
print("Class Reality SIMULATION=>" + str(reality))
print("Class RealityTabel SIMULATION=>" + str(reality_table))
self.publish_markers_new_data(pred_mod, reality, reality_table)
#self.rgb_camera_object.display_latest_image(int(camera_period*1000))
if self._grasp_activated == True:
if go_dustbin:
self.start_grasp_sequence_leave_dustbin(pred_mod)
else:
self.start_grasp_sequence(pred_mod)
rospy.loginfo("Start Prediction DONE...")
def start_camera_rgb_prediction_continuous(self, image_freq=20.0):
"""
It continuously make predictions
"""
# We reset the world Once
self.dynamic_world_service_call(self.change_env_request)
print("Waiting for Reset Env to settle=>")
wait_reset_period = 3.0
rospy.sleep(wait_reset_period)
print("Waiting for Reset Env to settle...DONE")
rate = rospy.Rate(image_freq)
life_time_ms = int((1.0 / image_freq) * 1000)
while not rospy.is_shutdown():
cv2_img = self.rgb_camera_object.get_latest_image()
self.rgb_camera_object.display_image(image_display=cv2_img,
life_time_ms=life_time_ms)
pred = self.predict_image(self.model, cv2_img)
# Add Z Axis value
z_value = 0.6 + 0.042
pred_mod = [pred[0], pred[1], z_value]
reality = self.get_xyz_from_world(self.model_to_track_name)
reality_table = self.get_xyz_from_world(self.table_to_track_name)
print("Class Prediction=>" + str(pred))
print("Class PredictionMod=>" + str(pred_mod))
print("Class Reality SIMULATION=>" + str(reality))
self.publish_markers_new_data(pred_mod, reality, reality_table)
rate.sleep()
rospy.loginfo("Start Prediction DONE...")
def move_endeffector(self, position_XYZ):
"""
Move the EndEffector to the position given, orientation set
"""
rospy.logwarn("START Move ENd Effector")
orientation_XYZW = [-0.707, 0.000, 0.707, 0.001]
result = self.fetch_move_client.move_endeffector(
position_XYZ, orientation_XYZW)
rospy.logwarn("END Move End Effector, result=" + str(result))
return result
def move_joints(self, joints_config):
"""
Move the joints to the specified configuration.
"""
rospy.logwarn("START Move Joints")
result = self.fetch_move_client.move_joints(joints_config)
rospy.logwarn("END Move Joints, result=" + str(result))
return result
def open_close_gripper(self, open_or_close):
"""
Choose if you want to open or close
"""
if open_or_close == "open":
# Open
gripper_x = 0.4
max_effort = 10.0
else:
# Close
gripper_x = 0.05
max_effort = 20.0
self.fetch_move_client.move_gripper(gripper_x, max_effort)
def check_if_object_grasped(self, tcp_xyz, wait_time=2.0, max_delta=0.2):
"""
It checks if the object to graps has been lifted from the table
"""
model_to_track_reality = self.get_xyz_from_world(
self.model_to_track_name)
print("model_to_track_reality=>" + str(model_to_track_reality))
print("tcp_xyz=>" + str(tcp_xyz))
z_model = model_to_track_reality[2]
z_tcp = tcp_xyz[2]
delta = z_tcp - z_model
rospy.logwarn("delta==" + str(delta) + " <= " + str(max_delta))
if delta <= max_delta:
grasp_state = "grasp_success"
else:
grasp_state = "grasp_fail"
#self.publish_new_grasp_state(grasp_state)
rospy.sleep(wait_time)
return grasp_state == "grasp_success"
def start_grasp_sequence(self, predicted_position_XYZ):
#Init Pose
self.fetch_move_client.go_to_safe_arm_pose()
# Set optimum torso height
height = 0.2
result = self.fetch_move_client.move_torso(height)
height_delta = 0.3
position_XYZ = [
predicted_position_XYZ[0], predicted_position_XYZ[1],
predicted_position_XYZ[2] + height_delta
]
orientation_XYZW = [-0.707, 0.000, 0.707, 0.001]
self.fetch_move_client.move_endeffector(position_XYZ, orientation_XYZW)
# Test Gripper
self.open_close_gripper(open_or_close="open")
self.open_close_gripper(open_or_close="close")
self.open_close_gripper(open_or_close="open")
# Lower ARM
height_delta = 0.19
position_XYZ = [
predicted_position_XYZ[0], predicted_position_XYZ[1],
predicted_position_XYZ[2] + height_delta
]
orientation_XYZW = [-0.707, 0.000, 0.707, 0.001]
self.fetch_move_client.move_endeffector(position_XYZ, orientation_XYZW)
# Close Gripper to grasp
self.open_close_gripper(open_or_close="close")
# Go Up with the object
height_delta = 0.3
position_XYZ = [
predicted_position_XYZ[0], predicted_position_XYZ[1],
predicted_position_XYZ[2] + height_delta
]
orientation_XYZW = [-0.707, 0.000, 0.707, 0.001]
self.fetch_move_client.move_endeffector(position_XYZ, orientation_XYZW)
# We check if we grapsed the object
self.check_if_object_grasped(tcp_xyz=position_XYZ)
# Go down with the object
height_delta = 0.19
position_XYZ = [
predicted_position_XYZ[0], predicted_position_XYZ[1],
predicted_position_XYZ[2] + height_delta
]
orientation_XYZW = [-0.707, 0.000, 0.707, 0.001]
self.fetch_move_client.move_endeffector(position_XYZ, orientation_XYZW)
# Release the object
self.open_close_gripper(open_or_close="open")
# We reset the Graps Marker
#self.publish_new_grasp_state(grasp_state="nothing")
# Up again
height_delta = 0.3
position_XYZ = [
predicted_position_XYZ[0], predicted_position_XYZ[1],
predicted_position_XYZ[2] + height_delta
]
orientation_XYZW = [-0.707, 0.000, 0.707, 0.001]
self.fetch_move_client.move_endeffector(position_XYZ, orientation_XYZW)
# Go to Init Safe pose
self.fetch_move_client.go_to_safe_arm_pose()
def start_grasp_sequence_leave_dustbin(self, predicted_position_XYZ):
#Init Pose
self.fetch_move_client.go_to_safe_arm_pose()
# Set optimum torso height
height = 0.2
result = self.fetch_move_client.move_torso(height)
height_delta = 0.3
position_XYZ = [
predicted_position_XYZ[0], predicted_position_XYZ[1],
predicted_position_XYZ[2] + height_delta
]
orientation_XYZW = [-0.707, 0.000, 0.707, 0.001]
self.fetch_move_client.move_endeffector(position_XYZ, orientation_XYZW)
# Test Gripper
self.open_close_gripper(open_or_close="open")
self.open_close_gripper(open_or_close="close")
self.open_close_gripper(open_or_close="open")
# Lower ARM
height_delta = 0.19
position_XYZ = [
predicted_position_XYZ[0], predicted_position_XYZ[1],
predicted_position_XYZ[2] + height_delta
]
orientation_XYZW = [-0.707, 0.000, 0.707, 0.001]
self.fetch_move_client.move_endeffector(position_XYZ, orientation_XYZW)
# Close Gripper to grasp
self.open_close_gripper(open_or_close="close")
# Go Up with the object
height_delta = 0.3
position_XYZ = [
predicted_position_XYZ[0], predicted_position_XYZ[1],
predicted_position_XYZ[2] + height_delta
]
orientation_XYZW = [-0.707, 0.000, 0.707, 0.001]
self.fetch_move_client.move_endeffector(position_XYZ, orientation_XYZW)
# We check if we grapsed the object
grasp_success = self.check_if_object_grasped(tcp_xyz=position_XYZ)
if grasp_success:
# The Graps Succeeeded We place object in DustBin
# Move to dustbin Pose
self.fetch_move_client.go_to_dustbin_arm_pose()
# Release the object
self.open_close_gripper(open_or_close="open")
# We reset the Graps Marker
#self.publish_new_grasp_state(grasp_state="nothing")
# Go to Init Safe pose
self.fetch_move_client.go_to_safe_arm_pose()