def attach_link(model1, link1, model2, link2):
req = AttachRequest()
req.model_name_1 = model1
req.link_name_1 = link1
req.model_name_2 = model2
req.link_name_2 = link2
client_attach.call(req)
def vacuum(self, model_name_1, model_name_2, action):
if model_name_1 == "UR1::urgripper":
self.actions[0] = action
elif model_name_1 == "UR2::urgripper":
self.actions[1] = action
elif model_name_1 == "UR3::urgripper":
self.actions[2] = action
elif model_name_1 == "UR4::urgripper":
self.actions[3] = action
elif model_name_1 == "ABB1::abbgripper":
self.actions[4] = action
elif model_name_1 == "ABB2::abbgripper":
self.actions[5] = action
elif model_name_1 == "ABB3::abbgripper":
self.actions[6] = action
elif model_name_1 == "ABB4::abbgripper":
self.actions[7] = action
req = AttachRequest()
req.model_name_1 = model_name_1
req.link_name_1 = "body"
req.model_name_2 = model_name_2
req.link_name_2 = "link"
if action:
attach_srv.call(req)
else:
detatch_srv.call(req)
def deactivate_gripper(self, model_name_2):
rospy.loginfo("Detach request received")
req = AttachRequest()
req.model_name_1 = 'edrone'
req.link_name_1 = 'base_frame'
req.model_name_2 = model_name_2
req.link_name_2 = 'link'
self._attach_srv_d.call(req)
def activate_gripper(self):
rospy.loginfo("Attach request received")
req = AttachRequest()
req.model_name_1 = 'edrone'
req.link_name_1 = 'base_frame'
req.model_name_2 = 'parcel_box'
req.link_name_2 = 'link'
self._attach_srv_a.call(req)
def detach_link(model1, link1, model2, link2):
detach_srv = rospy.ServiceProxy('/link_attacher_node/detach',Attach)
# detach_srv.wait_for_service()
req = AttachRequest()
req.model_name_1 = model1
req.link_name_1 = link1
req.model_name_2 = model2
req.link_name_2 = link2
detach_srv.call(req)
def deactivate_vacuum_gripper(self):
rospy.loginfo("Detach request received")
req = AttachRequest()
req.model_name_1 = self._vacuum_gripper_model_name
req.link_name_1 = self._vacuum_gripper_link_name
req.model_name_2 = self._object_model_name
req.link_name_2 = self._object_link_name
self._attach_srv_d.call(req)
def release(self, link_name):
parent = self.attach_link.rsplit('::')
child = link_name.rsplit('::')
req = AttachRequest()
req.model_name_1 = parent[0]
req.link_name_1 = parent[1]
req.model_name_2 = child[0]
req.link_name_2 = child[1]
res = self.detach_srv.call(req)
return res.ok
def vacuum(self, robot_name, obj_name, action):
req = AttachRequest()
req.model_name_1 = robot_name + '::' + 'gripper'
req.link_name_1 = "body"
req.model_name_2 = obj_name
req.link_name_2 = "link"
if action:
attach_srv.call(req)
else:
detatch_srv.call(req)
def detach(self):
attach_srv = rospy.ServiceProxy('/link_attacher_node/detach',Attach)
attach_srv.wait_for_service()
req = AttachRequest()
req.model_name_1 = "iris"
req.link_name_1 = "base_link"
req.model_name_2 = "apriltag"
req.link_name_2 = "link"
attach_srv.call(req)
def detach(self):
attach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Attach)
attach_srv.wait_for_service()
print("detaching")
req = AttachRequest()
req.model_name_1 = "iris_1"
req.link_name_1 = "base_link"
req.model_name_2 = "sample_probe"
req.link_name_2 = "base_link"
attach_srv.call(req)
def place(self):
req = AttachRequest()
req.model_name_1 = self.gripper
req.link_name_1 = self.gripper_link
req.model_name_2 = self.holding_object
req.link_name_2 = self.common_link
self.detach_srv.call(req)
time.sleep(2)
self.holding_object = ""
self.status = False
self.update_gripper_info(False)
print("Released Object")
return self.status
def detach_handler():
try:
s = rospy.ServiceProxy('/link_attacher_node/detach', Attach)
s.wait_for_service()
req = AttachRequest()
req.model_name_1 = "nut"
req.link_name_1 = "link_2"
req.model_name_2 = "youbot"
req.link_name_2 = "base_footprint"
s.call(req)
except rospy.ServiceException as e:
rospy.loginfo("Service failed")
def deactivate_vacuum_gripper(self):
rospy.set_param("vacuum_gripper_plugin_in_usage", True)
rospy.loginfo("Detach request received")
req = AttachRequest()
req.model_name_1 = self._vacuum_gripper_model_name
req.link_name_1 = self._vacuum_gripper_link_name
req.model_name_2 = self._object_model_name
req.link_name_2 = self._object_link_name
self._attach_srv_d.call(req)
rospy.set_param("vacuum_gripper_plugin_in_usage", False)
def attach(self):
print('Running attach_srv')
attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach)
attach_srv.wait_for_service()
print('Trying to attach')
req = AttachRequest()
req.model_name_1 = "iris"
req.link_name_1 = "base_link"
req.model_name_2 = "dummy_probe"
req.link_name_2 = "link"
attach_srv.call(req)
def detach(self):
attach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Attach)
attach_srv.wait_for_service()
print('Trying to detach')
req = AttachRequest()
req.model_name_1 = "iris"
req.link_name_1 = "base_link"
req.model_name_2 = "apriltag" # Insert the model name you want to attach to
req.link_name_2 = "link" # Insert link name you want to attach to
attach_srv.call(req)
print('Detached')
def attach(self):
print('Running attach_srv')
attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach)
attach_srv.wait_for_service()
print('Trying to attach')
req = AttachRequest()
req.model_name_1 = "iris"
req.link_name_1 = "base_link"
req.model_name_2 = "apriltag"
req.link_name_2 = "link"
attach_srv.call(req)
print('Attached')
self.mode="HOVER"
self.hover1(self.hover_loc1)
def release(self, link_name):
parent = self.attach_link.rsplit('::')
if '::' in link_name:
divider = '::'
elif '.' in link_name:
divider = '.'
else:
return False
child = link_name.rsplit(divider)
req = AttachRequest()
req.model_name_1 = parent[0]
req.link_name_1 = parent[1]
req.model_name_2 = child[0]
req.link_name_2 = child[1]
res = self.detach_srv.call(req)
return res.ok
def ativa_garra(letra):
"""!
Cria o link entre o drone e o pacote
Parametros:
letra: letra do pacote
"""
rospy.wait_for_service('/link_attacher_node/attach')
garra = rospy.ServiceProxy('/link_attacher_node/attach',Attach)
req = AttachRequest()
req.model_name_1 = "uav1"
req.link_name_1 = "base_link"
req.model_name_2 = "equipment"+letra
req.link_name_2 = "link_"+letra
garra(req)
def vacuum(self,robot_name,obj_name,action): if "ur" in robot_name: self.actions['ur']=action elif "sawyer" in robot_name: self.actions['sawyer']=action elif "abb" in robot_name: self.actions['abb']=action elif "staubli" in robot_name: self.actions['staubli']=action req = AttachRequest() req.model_name_1=robot_name+'::'+robot_name+'gripper' req.link_name_1 = "body" req.model_name_2 = obj_name req.link_name_2 = "link" if action: attach_srv.call(req) else: detatch_srv.call(req)
def pick(self, object):
self.gazebo_callback(object)
if self.grasping_condition(object):
# Pause physic environment
self.pause.call()
self.move_object(object)
time.sleep(1)
# Attach
req = AttachRequest()
req.model_name_1 = self.gripper
req.link_name_1 = self.gripper_link
req.model_name_2 = object
req.link_name_2 = self.common_link
self.attach_srv.call(req)
time.sleep(1)
self.holding_object = object
self.status = True
# Release physic environment
self.unpause.call()
time.sleep(1)
self.update_gripper_info(False)
print("Picked Object")
return self.status
if mag < 0.6 and degrees < 30:
rospy.loginfo(
"Creating ServiceProxy to /link_attacher_node/attach")
attach_srv = rospy.ServiceProxy('/link_attacher_node/attach',
Attach)
attach_srv.wait_for_service()
rospy.loginfo(
"Created ServiceProxy to /link_attacher_node/attach")
# Link them
rospy.loginfo("Attach drone to sample probe")
req = AttachRequest()
req.model_name_1 = "iris"
req.link_name_1 = "base_link"
req.model_name_2 = "sample_probe"
req.link_name_2 = "base_link"
attach_srv.call(req)
mode = None
elif mode == "DETACH":
rospy.loginfo(
"Creating ServiceProxy to /link_attacher_node/detach")
attach_srv = rospy.ServiceProxy('/link_attacher_node/detach',
Attach)
attach_srv.wait_for_service()
rospy.loginfo("Created ServiceProxy to /link_attacher_node/detach")
# Link them
rospy.loginfo("Detaching iris and sample_probe")
req = AttachRequest()
req.model_name_1 = "iris"
req.link_name_1 = "base_link"
#!/usr/bin/env python
import rospy
from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse
if __name__ == '__main__':
rospy.init_node('detach')
attach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Attach)
attach_srv.wait_for_service()
# Remove static link between cube and sawyer EEF
rospy.loginfo("Detaching cube1 and sawyer end effector")
req = AttachRequest()
req.model_name_1 = "cube1"
req.link_name_1 = "link"
req.model_name_2 = "sawyer"
req.link_name_2 = "right_l6"
attach_srv.call(req)
#!/usr/bin/env python
import rospy
from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse
if __name__ == '__main__':
rospy.init_node('demo_attach_links')
rospy.loginfo("Creating ServiceProxy to /link_attacher_node/attach")
attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach)
attach_srv.wait_for_service()
rospy.loginfo("Created ServiceProxy to /link_attacher_node/attach")
# Link them
rospy.loginfo("Attaching wall and cable")
req = AttachRequest()
req.model_name_1 = "wall_sockets"
req.link_name_1 = "wall"
req.model_name_2 = "cable"
req.link_name_2 = "plug"
attach_srv.call(req)
# From the shell:
"""
rosservice call /link_attacher_node/attach "model_name_1: 'cube1'
link_name_1: 'link'
model_name_2: 'cube2'
link_name_2: 'link'"
"""
if state == 0:
rospy.logerr('Pick up goal failed!: %s' %
client.get_goal_status_text())
rospy.sleep(0.05)
rospy.loginfo("Creating ServiceProxy to /link_attacher_node/attach")
attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach)
attach_srv.wait_for_service()
rospy.loginfo("Created ServiceProxy to /link_attacher_node/attach")
rospy.loginfo("Attaching finger and cube")
req = AttachRequest()
req.model_name_1 = "robot"
req.link_name_1 = "robotiq_85_right_finger_tip_link"
req.model_name_2 = "coke_can_box_model"
req.link_name_2 = "coke_can"
attach_srv.call(req)
scene.remove_world_object("part")
rospy.sleep(0.1)
place_pose = PoseStamped()
place_pose.header.frame_id = robot.get_planning_frame()
place_pose.pose.position.x = -0.224157
place_pose.pose.position.y = 0.704343
place_pose.pose.position.z = 0.175525
place_pose.pose.orientation.x = 0
place_pose.pose.orientation.y = 1
place_pose.pose.orientation.z = 0
place_pose.pose.orientation.w = 0
if __name__ == '__main__':
rospy.init_node('demo_attach_links')
rospy.loginfo("Creating ServiceProxy to /link_attacher_node/attach")
attach_srv = rospy.ServiceProxy('/link_attacher_node/attach',
Attach)
attach_srv.wait_for_service()
rospy.loginfo("Created ServiceProxy to /link_attacher_node/attach")
# Link them
rospy.loginfo("Attaching cube1 and cube2")
req = AttachRequest()
req.model_name_1 = "cube1"
req.link_name_1 = "link"
req.model_name_2 = "cube2"
req.link_name_2 = "link"
attach_srv.call(req)
# From the shell:
"""
rosservice call /link_attacher_node/attach "model_name_1: 'cube1'
link_name_1: 'link'
model_name_2: 'cube2'
link_name_2: 'link'"
"""
rospy.loginfo("Attaching cube2 and cube3")
req = AttachRequest()
req.model_name_1 = "cube2"
req.link_name_1 = "link"
req.model_name_2 = "cube3"
if __name__ == '__main__':
rospy.init_node('demo_attach_links')
rospy.loginfo("Creating ServiceProxy to /link_attacher_node/attach")
attach_srv = rospy.ServiceProxy('/link_attacher_node/attach',
Attach)
attach_srv.wait_for_service()
rospy.loginfo("Created ServiceProxy to /link_attacher_node/attach")
# Link them
rospy.loginfo("Attaching cube1 and cube2")
req = AttachRequest()
req.model_name_1 = "j2n6s300"
req.link_name_1 = "j2n6s300_link_6"
req.model_name_2 = "to_pickup_0"
req.link_name_2 = "link"
attach_srv.call(req)
# From the shell:
"""
rosservice call /link_attacher_node/attach "model_name_1: 'cube1'
link_name_1: 'link'
model_name_2: 'cube2'
link_name_2: 'link'"
"""
# rospy.loginfo("Attaching cube2 and cube3")
# req = AttachRequest()
# req.model_name_1 = "cube2"
# req.link_name_1 = "link"
# req.model_name_2 = "cube3"
import rospy
from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse
if __name__ == '__main__':
rospy.init_node('gripper_object_attach')
rospy.loginfo(
"Creating ServiceProxy to /link_attacher_node/attach and /link_attacher_node/attach"
)
attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach)
detach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Detach)
attach_srv.wait_for_service()
rospy.loginfo("Created ServiceProxy to /link_attacher_node/attach")
detach_srv.wait_for_service()
rospy.loginfo("Created ServiceProxy to /link_attacher_node/detach")
rospy.loginfo("Attaching object to gripper")
req = AttachRequest()
req.model_name_1 = "robot"
req.link_name_1 = "rg2_eef_link"
req.model_name_2 = "object"
req.link_name_2 = "object"
attach_srv.call(req)
rospy.loginfo("Detaching object from gripper")
req = AttachRequest()
req.model_name_1 = "robot"
req.link_name_1 = "rg2_eef_link"
req.model_name_2 = "object"
req.link_name_2 = "object"
attach_srv.call(req)
