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 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 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 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 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 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() 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 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 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 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 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 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 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
state = client.send_goal_and_wait(goal) cnt = cnt + 1 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
#!/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 cube1 and cube2") req = AttachRequest() req.model_name_1 = "robot" req.link_name_1 = "left_link_6" req.model_name_2 = "test_box" 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'" """
#!/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 gripper and red box.") req = AttachRequest() req.model_name_1 = "mogi_arm" req.link_name_1 = "left_finger" req.model_name_2 = "red_box" req.link_name_2 = "link" attach_srv.call(req) # From command line: """ rosservice call /link_attacher_node/attach "model_name_1: 'cube1' link_name_1: 'link' model_name_2: 'cube2' link_name_2: 'link'" """
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 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"
# 15 degree threshold and magnitude is less than 0.6 meters, attach works 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()
pose_goal.position.y = posy pose_goal.position.z = 0.045 move_group.set_pose_target(pose_goal) plan = move_group.go(wait=True) move_group.stop() move_group.clear_pose_targets() #close the gripper using effort-controller print("close gripper") moveFingers("close",[-0.5,-0.5,-0.5],'j2s7s300',3) #attach the block to the end-effector req = AttachRequest() req.model_name_1 = "j2s7s300" req.link_name_1 = "j2s7s300_link_7" req.model_name_2 = item_name req.link_name_2 = "link" attach_srv.call(req) #RAISE ARM BACK UP IN SAME X-Y POSITION pose_goal.orientation.w = 0 pose_goal.orientation.x = 1 pose_goal.orientation.y = 0 pose_goal.orientation.z = 0 pose_goal.position.x = posx pose_goal.position.y = posy pose_goal.position.z = z_height_reset
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("Applying brakes to ebot") req = AttachRequest() req.model_name_1 = "ebot" req.link_name_1 = "ebot_base" req.model_name_2 = "ground_plane" 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"
#!/usr/bin/env python # -*- coding: utf-8 -*- import sys import rospy import random import actionlib from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown from geometry_msgs.msg import PoseStamped from std_msgs.msg import Float64, MultiArrayDimension from control_msgs.msg import GripperCommandAction, FollowJointTrajectoryAction 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") rospy.loginfo("Attaching finger and cube") req = AttachRequest() req.model_name_1 = "robot" req.link_name_1 = "bh_finger_11_link" req.model_name_2 = "coke_can_box_model" req.link_name_2 = "coke_can" attach_srv.call(req)
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 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"
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 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"
#!/usr/bin/env python import rospy from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse if __name__ == '__main__': rospy.init_node('demo_detach_links') 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 cube1 and cube2") req = AttachRequest() req.model_name_1 = "robot" req.link_name_1 = "right_link_6" req.model_name_2 = "unit_box" req.link_name_2 = "link" attach_srv.call(req) # From the shell: """ rosservice call /link_attacher_node/detach "model_name_1: 'cube1' link_name_1: 'link' model_name_2: 'cube2' link_name_2: 'link'" """