def find_node(wanted_node_name, unique=False): ''' Do a lookup to find a node with the given name. The given name is treated as the unresolved node name. Hence this lookup will find nodes with the same name, but different namespaces. This will raise exceptions, if the node couldn't be found or in case unique is set multiple nodes with the same name are found. :param str wanted_node_name: unresolved name of the node looked for (e.g. 'gateway', not '/concert/gateway') :returns: the fully resolved name of the node (unique) or list of fully resolved names (non-unique) :rtype: str :raises: :exc:`.NotFoundException` :todo: accept resolved names -> https://github.com/robotics-in-concert/rocon_tools/issues/30 ''' available_nodes = rosnode.get_node_names() found_nodes = [] for resolved_node_name in available_nodes: node_name = resolved_node_name[(resolved_node_name.rfind('/') + 1):len(resolved_node_name)] if node_name == wanted_node_name: found_nodes.append(resolved_node_name) if len(found_nodes) == 0: raise NotFoundException("Node '" + str(wanted_node_name) + "' not found.") if unique: if len(found_nodes) > 1: raise NotFoundException('More then one node with the same name found:' + str(found_nodes)) return found_nodes
def is_node_up(node): try: node_up = any([node in upnode for upnode in rosnode.get_node_names(namespace)]) return node_up except Exception: return False
def say(robot, msg, callback = None, feedback =None): """ Says loudly the message. Several TTS systems are tested: - first, try the Acapela TTS (through the acapela-ros Genom module) - then the ROS 'sound_play' node - eventually, the Genom 'textos' module :param msg: a text to say. """ def execute(robot): logger.info("Robot says: " + msg) if robot.supports(ROS): import roslib; roslib.load_manifest('sound_play') import rospy, os, sys from sound_play.libsoundplay import SoundClient soundhandle = SoundClient() soundhandle.say(msg) return (True, None) elif robot.hasmodule("textos"): return robot.execute([ genom_request( "textos", "Say", [msg], wait_for_completion = False if callback else True, callback = callback)]) else: logger.warning("No ROS, no textos module: can not do speech synthesis.") return (True, None) if robot.supports(ROS): import rosnode nodes = rosnode.get_node_names() if "/acapela" in nodes: import actionlib from acapela.msg import SayGoal, SayAction # use Acapela TTS client = actionlib.SimpleActionClient('/acapela/Say', SayAction) ok = client.wait_for_server() if not ok: print("Could not connect to the Acapela ROS action server! Aborting action") return # Creates a goal to send to the action server. goal = SayGoal() goal.message = msg return [ros_request(client, goal, wait_for_completion = False if callback else True, callback = callback, feedback=feedback )] # Return a non-blocking action. Useful to be able to cancel it later! return [python_request(execute)]
def get_all_node_info(self): infos = [] self.remove_dead_nodes() for node_name in rosnode.get_node_names(): info = self.get_node_info(node_name) if info is not False: infos.append((node_name, info)) return infos
def main(argv): print "REFLEX GRIPPER RR BRIDGE" print "========================" # Parse command line arguments parser = argparse.ArgumentParser( description='Initialize a Gripper Hand') parser.add_argument('--port', type=int, default=0, help='TCP port to host service on' +\ '(will auto-generate if not specified)') parser.add_argument('name', help='The desired name for the gripper.' +\ 'Must match name given to start gripper services(e.g. left)') args = parser.parse_args(argv) # Check for valid name name = args.name if not "/reflex_%s"%(name) in rosnode.get_node_names(): print "The name '%s' is not valid. The gripper services may not "%(name) +\ "be started yet." raw_input("Press ENTER to close") return -1 # Enable numpy RR.RobotRaconteurNode.s.UseNumPy = True # Set the node name RR.RobotRaconteurNode.s.NodeName = "ReflexGripperServer.%s"%name # Initialize the object gripper_obj = Gripper_impl(name) # Create transport, register it, and start the server print "Registering Transport..." t = RR.TcpTransport() t.EnableNodeAnnounce(RR.IPNodeDiscoveryFlags_NODE_LOCAL | RR.IPNodeDiscoveryFlags_LINK_LOCAL | RR.IPNodeDiscoveryFlags_SITE_LOCAL) RR.RobotRaconteurNode.s.RegisterTransport(t) t.StartServer(args.port) port = args.port if (port == 0): port = t.GetListenPort() # Register the service type and the service print "Starting Service..." RR.RobotRaconteurNode.s.RegisterServiceType(gripper_servicedef) RR.RobotRaconteurNode.s.RegisterService("Gripper", "ReflexGripper_Interface.Gripper", gripper_obj) print "Service started, connect via" print "tcp://localhost:%s/ReflexGripperServer.%s/Gripper"%(port, name) raw_input("press ENTER to quit ... \r\n") gripper_obj.close() # This must be here to prevent segfault RR.RobotRaconteurNode.s.Shutdown()
def is_running(self): if self._subprocess is not None: return True try: return True if '/rosbridge_websocket' in rosnode.get_node_names( ) else False except: return False
def get_node_names(self): """ Gets a list of available services via a ros service call. :returns: a list of all nodes that provide the set_logger_level service, ''list(str)'' """ set_logger_level_nodes = [] nodes = rosnode.get_node_names() for name in sorted(nodes): for service in rosservice.get_service_list(name): if service == name + '/set_logger_level': set_logger_level_nodes.append(name) return set_logger_level_nodes
def test_long_viseme(self): filename = get_rosbag_file("long_viseme") # job = play_rosbag(filename) # job.join() pub, msg_class = rostopic.create_publisher("/blender_api/queue_viseme", "blender_api_msgs/Viseme", True) bag = rosbag.Bag(filename) duration = bag.get_end_time() - bag.get_start_time() fps = bag.get_message_count() / float(duration) wait = 1.0 / fps / 10 # 10 times faster than the speed of the msg recoreded for topic, msg, _ in rosbag_msg_generator(filename): pub.publish(msg) time.sleep(wait) # Test if blender is still alive self.assertIn("/blender_api", rosnode.get_node_names()) self.assertTrue(any(["blender_api" in name for name in self.runner.pm.get_active_names()]))
def get_node_info(self, event): """ Get the list of all Nodes running on the host. Update the __node_list """ if not self.__is_enabled: pass if self.__is_enabled: nodes = [] for node_name in rosnode.get_node_names(): try: node_api = rosnode.get_api_uri(rospy.get_master(), node_name, skip_cache=True) if self._id in node_api[2]: nodes.append(node_name) except : pass for node in nodes: if node not in self.__node_list: """TODO currently not catching the exception here - master not running is a hard error so it does not make sense to continue running..""" node_api = rosnode.get_api_uri(rospy.get_master(), node, skip_cache=True) try: pid = self.get_node_pid(node_api, node) if not pid: continue node_process = psutil.Process(pid) new_node = NodeStatisticsHandler( self._id, node, node_process) self.__dict_lock.acquire(True) self.__node_list[node] = new_node self.__dict_lock.release() except psutil.NoSuchProcess: rospy.loginfo('pid of node %s could not be fetched' % node) continue self.__dict_lock.acquire() to_remove = [] for node_name in self.__node_list: if node_name not in nodes: rospy.logdebug('removing %s from host' % node_name) to_remove.append(node_name) for node_name in to_remove: self.remove_node(node_name) self.__dict_lock.release()
def check_registration(e): """\ Shuts down this ROS node if it is not registered on the master. This will effectively kill the director each time the ROS master is restarted, preventing silent and subtle publishing failure. This should cause a shutdown *only* if the master can be contacted and the node is not registered. """ import rosnode try: nodes = rosnode.get_node_names() except rosnode.ROSNodeIOException: rospy.logdebug("Could not contact master for registration check") return if rospy.get_name() not in nodes: rospy.logwarn("Node no longer registered, shutting down") rospy.signal_shutdown("Node no longer registered") os.kill(os.getpid(), signal.SIGTERM)
def checkNodeState(target_node_name, needed, sub_success="", sub_fail=""): nodes = rosnode.get_node_names() if target_node_name in nodes: if needed: okMessage("Node " + target_node_name + " exists") if sub_success: print Fore.GREEN + " " + sub_success + Fore.RESET else: errorMessage("Node " + target_node_name + " exists unexpecetedly. This should be killed with rosnode kill") if sub_fail: print Fore.RED + " " + sub_fail + Fore.RESET else: if needed: errorMessage("Node " + target_node_name + " doesn't exists. This node is NEEDED") if sub_fail: print Fore.RED + " " + sub_fail + Fore.RESET else: okMessage("Node " + target_node_name + " doesn't exists") if sub_success: print Fore.GREEN + " " + sub_success + Fore.RESET
def test_rosnode_list(self): import rosnode cmd = 'rosnode' nodes = ['/talker', '/foo/talker', '/bar/talker', '/baz/talker1', '/baz/talker2', '/baz/talker3', '/rosout', ] l = rosnode.get_node_names() for t in nodes: self.assert_(t in l) try: rosnode._rosnode_cmd_list([cmd, 'list', 'one', 'two']) self.fail("should have failed") except SystemExit, e: self.assertNotEquals(0, e.code)
def handle_pose(self,msg): #print "MESSAGE", dir(msg) pos = msg.pose.pose.position if self.get_home: if self.copter_name == rosnode.get_node_names()[0]: GLOB_X = pos.x GLOB_Y = pos.y self.home_x = GLOB_X #pos.x self.home_y = GLOB_Y #pos.y self.get_home = False q = msg.pose.pose.orientation ang = q.x #print pos.x, self.home_x #print "X ",q.x, " Y ", q.y, " Z ", q.z, " W ", q.w br.sendTransform(( pos.x-self.home_x, pos.y-self.home_y, pos.z), (q.x, q.y, q.z, q.w), rospy.Time.now(), self.copter_name, "fcu")
def getROSNodes(self): nodes = [] try: nodenames = rosnode.get_node_names(None) except Exception as inst: print "ERROR:[getROSNodes-01] ", inst return nodes for nodename in nodenames: #rosnode.rosnode_info(nodename) api = rosnode.get_api_uri(rosgraph.Master("/rosnode"), nodename) if api: try: node = ServerProxy(api) code, msg, pid = node.getPid("/rosnode") if code == 1: res = re.search("^(.*)_[0-9]+$", nodename) while res is not None: nodename = res.group(1) res = re.search("^(.*)_[0-9]+$", nodename) nodes.append((nodename, pid)) except: pass return nodes
def __init__(self, parent): super(NodeSelection, self).__init__() self.parent_widget = parent self.selected_nodes = [] self.setWindowTitle("Select the nodes you want to record") self.resize(500, 700) self.area = QScrollArea(self) self.main_widget = QWidget(self.area) self.ok_button = QPushButton("Done", self) self.ok_button.clicked.connect(self.onButtonClicked) self.ok_button.setEnabled(False) self.main_vlayout = QVBoxLayout(self) self.main_vlayout.addWidget(self.area) self.main_vlayout.addWidget(self.ok_button) self.setLayout(self.main_vlayout) self.selection_vlayout = QVBoxLayout(self) self.node_list = rosnode.get_node_names() self.node_list.sort() for node in self.node_list: self.addCheckBox(node) self.main_widget.setLayout(self.selection_vlayout) self.show()
def __init__(self): #clean up previous process os.system("killall gzserver gzclient") if rosgraph.is_master_online( ): # Checks the master uri and results boolean (True or False) print 'ROS MASTER is active' nodes = rosnode.get_node_names() if "/rviz" in nodes: print("Rviz active") rvizflag = " rviz:=false" else: rvizflag = " rviz:=true" #start ros impedance controller uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) self.launch = roslaunch.parent.ROSLaunchParent(uuid, [ os.environ['LOCOSIM_DIR'] + "/ros_impedance_controller/launch/ros_impedance_controller.launch" ]) #only available in ros lunar # roslaunch_args=rvizflag # self.launch = roslaunch.parent.ROSLaunchParent(uuid, [os.environ['LOCOSIM_DIR'] + "/ros_impedance_controller/launch/ros_impedance_controller_stdalone.launch"],roslaunch_args=[roslaunch_args]) self.launch.start() ros.sleep(4.0) threading.Thread.__init__(self) # instantiate graphic utils self.ros_pub = RosPub(True) self.joint_names = "" self.u = Utils() self.comPoseW = np.zeros(6) self.baseTwistW = np.zeros(6) self.stance_legs = np.array([True, True, True, True]) self.q = np.zeros(12) self.qd = np.zeros(12) self.tau = np.zeros(12) self.q_des = np.zeros(12) self.qd_des = np.zeros(12) self.tau_ffwd = np.zeros(12) self.b_R_w = np.eye(3) self.grForcesW = np.zeros(12) self.basePoseW = np.zeros(6) self.J = [np.eye(3)] * 4 self.wJ = [np.eye(3)] * 4 self.robot_name = ros.get_param('/robot_name') self.sub_contact = ros.Subscriber("/" + self.robot_name + "/contacts_state", ContactsState, callback=self._receive_contact, queue_size=100) self.sub_pose = ros.Subscriber("/" + self.robot_name + "/ground_truth", Odometry, callback=self._receive_pose, queue_size=1) self.sub_jstate = ros.Subscriber("/" + self.robot_name + "/joint_states", JointState, callback=self._receive_jstate, queue_size=1) self.pub_des_jstate = ros.Publisher("/command", JointState, queue_size=1) # freeze base and pause simulation service self.reset_world = ros.ServiceProxy('/gazebo/set_model_state', SetModelState) self.reset_gravity = ros.ServiceProxy('/gazebo/set_physics_properties', SetPhysicsProperties) self.pause_physics_client = ros.ServiceProxy('/gazebo/pause_physics', Empty) self.unpause_physics_client = ros.ServiceProxy( '/gazebo/unpause_physics', Empty) # Loading a robot model of HyQ (Pinocchio) self.robot = getRobotModel("hyq") #send data to param server self.verbose = conf.verbose self.u.putIntoGlobalParamServer("verbose", self.verbose)
self.ros_notificator.shutdown() except Exception, e: logger.error("The ROS notificator could not be shut down") logger.exception(e) SimUtil.delete_simulation_dir() # Cleanup ROS core nodes, services, and topics (the command should be almost # instant and exit, but wrap it in a timeout since it's semi-officially supported) logger.info("Cleaning up ROS nodes and services") for param in rospy.get_param_names(): if param not in self._initial_ros_params: rospy.delete_param(param) for node in rosnode.get_node_names(): if node not in self._initial_ros_nodes: os.system('rosnode kill ' + str(node)) try: res = subprocess.check_output(["rosnode", "list"]) if res.find("/gazebo") > -1 and res.find("/Watchdog") > -1: os.system('rosnode kill /gazebo /Watchdog') elif res.find("/gazebo") > -1: os.system('rosnode kill /gazebo >/dev/null 2>&1') elif res.find("/Watchdog") > -1: os.system('rosnode kill /Watchdog >/dev/null 2>&1') except Exception, e:
def test_node_exist(self): nodes = rosnode.get_node_names() self.assertIn('/motors', nodes, "node does not exist")
def test_rosnode_list(self): import rosnode cmd = 'rosnode' nodes = ['/talker', '/foo/talker', '/bar/talker', '/baz/talker1', '/baz/talker2', '/baz/talker3', '/rosout', ] l = rosnode.get_node_names() for t in nodes: self.assert_(t in l) try: rosnode._rosnode_cmd_list([cmd, 'list', 'one', 'two']) self.fail("should have failed") except SystemExit as e: self.assertNotEquals(0, e.code) with fakestdout() as b: rosnode._rosnode_cmd_list([cmd, 'list']) self._check(nodes, tolist(b)) with fakestdout() as b: rosnode._rosnode_cmd_list([cmd, 'list', '/']) l = tolist(b) self._check(nodes, l) num_nodes = len(l) # test -u uris with fakestdout() as b: rosnode._rosnode_cmd_list([cmd, 'list', '-u', '/']) l = tolist(b) self.assertEquals(num_nodes, len(l)) self.failIf([n for n in l if not n.startswith('http://')]) # test -a all with fakestdout() as b: rosnode._rosnode_cmd_list([cmd, 'list', '-a', '/']) l = tolist(b) uris = [x.split()[0] for x in l if x] names = [x.split()[1] for x in l if x] self._check(nodes, names) self.assertEquals(num_nodes, len(uris)) self.failIf([n for n in uris if not n.startswith('http://')]) # test with namespace foon = [p for p in nodes if p.startswith('/foo/')] not_foon = [p for p in nodes if not p.startswith('/foo/')] for ns in ['foo', '/foo', '/foo/']: with fakestdout() as b: rosnode._rosnode_cmd_list([cmd, 'list', ns]) self._check(foon, tolist(b)) self._notcheck(not_foon, tolist(b)) bazn = [p for p in nodes if p.startswith('/baz/')] not_bazn = [p for p in nodes if not p.startswith('/baz/')] for ns in ['baz', '/baz', '/baz/']: with fakestdout() as b: rosnode._rosnode_cmd_list([cmd, 'list', ns]) self._check(bazn, tolist(b)) self._notcheck(not_bazn, tolist(b)) # test with no match with fakestdout() as b: rosnode._rosnode_cmd_list([cmd, 'list', '/not/a/namespace/']) self.assertEquals([], tolist(b))
def main(): rospy.init_node("crane_x7_pick_and_place_controller") robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") def move_max_velocity(value=0.5): arm.set_max_velocity_scaling_factor(value) gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) print("Group names:") print(robot.get_group_names()) print("Current state:") print(robot.get_current_state()) ### ### b-group ### #紙Aコップのx座標 紙Aコップのy座標 紙Bコップx座標 紙Bコップy座標 position_base = [[0.30, 0.13, 0.30, -0.11]] Acup_tukamu = False Bcup_tukamu = False """ 使い方 ハンドをつかんだ時は position_manager(False,False,x,y,True) ハンドをはなした時は position_manager(False,False,x,y,False) Aカップをつかみたい時の手先の位置を知りたいときは aa = position_manager(True,True,0,0,False) x = aa[0] y = aa[1] BBカップをつかみたい時の手先の位置を知りたいときは aa = position_manager(True,False,0,0,False) x = aa[0] y = aa[1] """ def position_manager(master_judge, paper_cup, x, y, tukami): global Acup_tukamu global Bcup_tukamu position_ret = [0.0, 0.0] if master_judge == True: if len(position_base) > 0: #ホンスワン if paper_cup == True: position_ret[0] = position_base[len(position_base) - 1][2] position_ret[1] = position_base[len(position_base) - 1][3] #position_historyの末尾の紙Aコップのx座標y座標 return position_ret else: position_ret[0] = position_base[len(position_base) - 1][0] position_ret[1] = position_base[len(position_base) - 1][1] #position_historyの末尾の紙Bコップのx座標y座標 return position_ret else: return position_ret else: #熊谷さん #position_base配列の末尾にx,yを追加 if tukami == True: #Aをつかんでいる場合 if position_base[len(position_base) - 1][0] == x and position_base[ len(position_base) - 1][1] == y: position_base.append([ x, y, position_base[len(position_base) - 1][2], position_base[len(position_base) - 1][3] ]) Acup_tukamu = True #Bをつかんでいる場合 elif position_base[len(position_base) - 1][2] == x and position_base[ len(position_base) - 1][3] == y: position_base.append([ position_base[len(position_base) - 1][0], position_base[len(position_base) - 1][1], x, y ]) Bcup_tukamu = True else: #Aをはなした時 if Acup_tukamu == True: position_base.append([ x, y, position_base[len(position_base) - 1][2], position_base[len(position_base) - 1][3] ]) Acup_tukamu = False elif Bcup_tukamu == True: position_base.append([ position_base[len(position_base) - 1][0], position_base[len(position_base) - 1][1], x, y ]) Bcup_tukamu = False return position_ret ### ### b-group-end ### ### ### a-group ### # アーム初期ポーズを表示 arm_initial_pose = arm.get_current_pose().pose print("Arm initial pose:") print(arm_initial_pose) # ハンドを開く/ 閉じる def move_gripper(pou): gripper.set_joint_value_target([pou, pou]) gripper.go() # アームを移動する def move_arm(pos_x, pos_y, pos_z): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x target_pose.position.y = pos_y target_pose.position.z = pos_z q = quaternion_from_euler(-3.14 / 2.0, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # コップ上部をつかむ位置へ移動1 def move_arm_upper(pos_x, pos_y): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x - 0.13 target_pose.position.y = pos_y target_pose.position.z = 0.075 q = quaternion_from_euler(-3.14 / 2.0, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # コップ上部をつかむ位置へ移動2 def move_arm_upper_catch(pos_x, pos_y): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x - 0.005 target_pose.position.y = pos_y target_pose.position.z = 0.075 q = quaternion_from_euler(-3.14 / 2.0, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # コップ下部をつかむ位置へ移動1 def move_arm_lower(pos_x, pos_y): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x - 0.13 target_pose.position.y = pos_y target_pose.position.z = -0.01 q = quaternion_from_euler(-3.14 / 2.0 - 0.2, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # コップ下部をつかむ位置へ移動2 def move_arm_lower_catch(pos_x, pos_y): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x - 0.005 target_pose.position.y = pos_y target_pose.position.z = -0.01 q = quaternion_from_euler(-3.14 / 2.0 - 0.2, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # コップの上を持ち上へ移動 def move_arm_upper_up(pos_x, pos_y): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x - 0.005 target_pose.position.y = pos_y target_pose.position.z = 0.075 + 0.1 q = quaternion_from_euler(-3.14 / 2.0, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # コップの下を持ち上へ移動 def move_arm_lower_up(pos_x, pos_y): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x - 0.005 target_pose.position.y = pos_y target_pose.position.z = -0.01 + 0.1 q = quaternion_from_euler(-3.14 / 2.0, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 ### ### a-group-end ### # SRDFに定義されている"home"の姿勢にする arm.set_named_target("home") arm.go() #ハンドを開く move_gripper(1.3) #コップA下部を掴む-----1 #流れ 1,2,3,4,5,6,7,8,9 #aaにコップAの(x,y)を代入 aa = position_manager(True, True, 0, 0, False) #グリッパを開く move_gripper(1.3) #アームをコップの下部と水平な部分へ移動 move_arm_lower(aa[0], aa[1]) #アームをコップの下部とくっつける move_arm_lower_catch(aa[0], aa[1]) #グリッパを閉じ、コップをつかむ move_gripper(0.28) #アームを持ち上げる(空中で停止させるため複数回呼び出している) move_arm_lower_up(aa[0], aa[1]) move_arm_lower_up(aa[0], aa[1]) move_arm_lower_up(aa[0], aa[1]) move_arm_lower_up(aa[0], aa[1]) #アームを下げる move_arm_lower_catch(aa[0], aa[1]) #グリッパを開き、コップを放す move_gripper(1.3) #アームを持ち上げコップから離れたらホームへ戻る move_arm_lower_up(aa[0], aa[1]) move_max_velocity() arm.set_named_target("home") arm.go() #コップB上部を掴む-----2 #流れ 10,11,12,13,14,15,16,17,18,19,20 #aaにコップBの(x,y)を代入 aa = position_manager(True, False, 0, 0, False) #グリッパを開く move_gripper(1.3) #アームをコップの下部と水平な部分へ移動 move_arm_upper(aa[0], aa[1]) #アームをコップの下部とくっつける move_arm_upper_catch(aa[0], aa[1]) #グリッパを閉じ、コップをつかむ move_gripper(0.28) move_arm_upper_up(aa[0], aa[1]) #アームを持ち上げる(空中で停止させるため複数回呼び出している) move_arm_upper_up(aa[0], aa[1]) move_arm_upper_up(aa[0], aa[1]) move_arm_upper_up(aa[0], aa[1]) #アームをボールの真上へ(空中で停止させるため複数回呼び出している) move_arm_upper_up(aa[0], aa[1] + 0.1) move_arm_upper_up(aa[0], aa[1] + 0.1) move_arm_upper_up(aa[0], aa[1] + 0.1) move_arm_upper_up(aa[0], aa[1] + 0.1) #アームを下げカップをボールにかぶせる move_arm_upper_catch(aa[0], aa[1] + 0.1) #元の位置へ戻す move_arm_upper_catch(aa[0], aa[1]) #グリッパを開き、コップを放す move_gripper(1.3) #アームを持ち上げコップから離れたらホームへ戻る move_arm_upper_up(aa[0], aa[1]) move_max_velocity() arm.set_named_target("home") arm.go() #パフォーマンスするならここ #取り敢えず先輩のコードをそのまま流用 #頂点に振り上げる move_arm(0.1, 0.0, 0.5) #あおり move_arm(0.22, -0.2, 0.2) #頂点に振り上げる move_arm(0.1, 0.0, 0.5) #あおり move_arm(0.22, 0.2, 0.2) #homeに戻る arm.set_named_target("home") arm.go() #コップB下部を掴む-----4 #瞬間移動 1,2,3,4,5,6,7,8,9 #aaにコップBの(x,y)を代入 aa = position_manager(True, False, 0, 0, False) #グリッパを開く move_gripper(1.3) #アームをコップの下部と水平な部分へ移動 move_arm_lower(aa[0], aa[1]) #アームをコップの下部とくっつける move_arm_lower_catch(aa[0], aa[1]) #グリッパを閉じ、コップをつかむ move_gripper(0.28) #アームを持ち上げる(空中で停止させるため複数回呼び出している) move_arm_lower_up(aa[0], aa[1]) move_arm_lower_up(aa[0], aa[1]) move_arm_lower_up(aa[0], aa[1]) move_arm_lower_up(aa[0], aa[1]) #アームを下げる move_arm_lower_catch(aa[0], aa[1]) #グリッパを開き、コップを放す move_gripper(1.3) #アームを持ち上げコップから離れたらホームへ戻る move_arm_lower_up(aa[0], aa[1]) move_max_velocity() arm.set_named_target("home") arm.go() #コップA上部を掴む-----4 #瞬間移動 10,11,12,13,14,15,16,17 #aaにコップAの(x,y)を代入 aa = position_manager(True, True, 0, 0, False) #グリッパを開く move_gripper(1.3) #アームをコップの上部と水平な部分へ移動 move_arm_upper(aa[0], aa[1]) #アームをコップの上部とくっつける move_arm_upper_catch(aa[0], aa[1]) #グリッパを閉じ、コップをつかむ move_gripper(0.28) move_arm_upper_up(aa[0], aa[1]) #アームを持ち上げる(空中で停止させるため複数回呼び出している) move_arm_upper_up(aa[0], aa[1]) move_arm_upper_up(aa[0], aa[1]) move_arm_upper_up(aa[0], aa[1]) move_arm_upper_up(aa[0], aa[1]) #アームを下げる move_arm_upper_catch(aa[0], aa[1]) #グリッパを開き、コップを放す move_gripper(1.3) #アームを持ち上げコップから離れたらホームへ戻る move_arm_upper_up(aa[0], aa[1]) move_max_velocity() arm.set_named_target("home") arm.go() #コップAの上部をつかむ-----5 #中身の確認 #aaにコップAの(x,y)を代入 aa = position_manager(True, True, 0, 0, False) #グリッパを開く move_gripper(1.3) #アームをコップの上部と水平な部分へ移動 move_arm_upper(aa[0], aa[1]) #アームをコップの上部とくっつける move_arm_upper_catch(aa[0], aa[1]) #グリッパを閉じ、コップをつかむ move_gripper(0.28) move_arm_upper_up(aa[0], aa[1]) #アームを持ち上げる(空中で停止させるため複数回呼び出している) move_arm_upper_up(aa[0], aa[1]) move_arm_upper_up(aa[0], aa[1]) move_arm_upper_up(aa[0], aa[1]) move_arm_upper_up(aa[0], aa[1]) #アームを手前に移動 move_arm_upper_up(aa[0] - 0.13, aa[1]) #アームを下げる move_arm_upper(aa[0], aa[1]) #グリッパを開き、コップを放す move_gripper(1.3) #アームを持ち上げコップから離れたらホームへ戻る move_arm_upper_up(aa[0], aa[1]) move_max_velocity() arm.set_named_target("home") arm.go() #コップB下部を掴む-----6 #中身の確認 #aaにコップBの(x,y)を代入 aa = position_manager(True, False, 0, 0, False) #グリッパを開く move_gripper(1.3) #アームをコップの下部と水平な部分へ移動 move_arm_lower(aa[0], aa[1]) #アームをコップの下部とくっつける move_arm_lower_catch(aa[0], aa[1]) #グリッパを閉じ、コップをつかむ move_gripper(0.28) #アームを持ち上げる(空中で停止させるため複数回呼び出している) move_arm_lower_up(aa[0], aa[1]) move_arm_lower_up(aa[0], aa[1]) move_arm_lower_up(aa[0], aa[1]) move_arm_lower_up(aa[0], aa[1]) #アームを手前に移動 move_arm_lower_up(aa[0] - 0.13, aa[1]) #アームを下げる move_arm_lower(aa[0], aa[1]) #グリッパを開き、コップを放す move_gripper(1.3) #アームを持ち上げコップから離れたらホームへ戻る move_arm_lower_up(aa[0], aa[1]) move_max_velocity() arm.set_named_target("home") arm.go()
def add_any_pipeline_rois(self): # TODO does the namespace argument do what i want? ns = rospy.get_namespace() nodes = rosnode.get_node_names(namespace=ns) # TODO delete me #rospy.logwarn('nodes w/ namespace=' + ns + ':' + str(nodes)) #rospy.logwarn('nodes w/o namespace specified:' + str(rosnode.get_node_names())) roi_params = [['circular_mask_x', 'circular_mask_y', 'circular_mask_r'], \ ['roi_points'], ['roi_l', 'roi_r', 'roi_b', 'roi_t']] new_nodes = False for n in nodes: if 'tracker_' in n: def getter(p): return rospy.get_param(n + '/' + p) else: continue node_num = int(n.split('_')[-1]) if not any(map(lambda d: node_num in d, [self.circular_rois, self.polygonal_rois, \ self.rectangular_rois])): new_nodes = True break if not new_nodes: return self.clear_rois() for n in nodes: if 'tracker_' in n: def getter(p): return rospy.get_param(n + '/' + p) else: continue for params in roi_params: # may want to replace with rosnode API instrospection at some point # provided it is stable / well documented enough try: # TODO need to prepend namespace? # TODO TODO fix whatever causing roi_points get to be unreachable # that is the one that is set, but it just tries for roi_l! node_num = int(n.split('_')[-1]) self.add_roi(node_num, params, get_fn=getter) # if the above roi is added successfully # we will make it to this line, otherwise # we will end up in the except block self.start_subscribers(node_num) except KeyError: pass if any(map(lambda x: len(x) > 0, [self.circular_rois, self.polygonal_rois, \ self.rectangular_rois])): self.have_rois = True else: self.have_rois = False # TODO TODO do some caching / checking to not have to recompute this everytime self.image_mask = None
def onMessage(self, payload, isBinary): # Debug if isBinary: print("Binary message received: {0} bytes".format(len(payload))) else: # print("Text message received: {0}".format(payload.decode('utf8'))) # Do stuff # pub = rospy.Publisher('blockly', String, queue_size=10) # time.sleep(1) # pub.publish("blockly says: "+payload.decode('utf8')) # Simple text protocol for communication # first line is the name of the method # next lines are body of message message_text = payload.decode('utf8') message_data = message_text.split('\n', 1) if len(message_data) > 0: method_name = message_data[0] if len(message_data) > 1: method_body = message_data[1] if method_name.startswith('play'): CodeStatus.set_current_status(CodeStatus.RUNNING) BlocklyServerProtocol.build_ros_node(method_body) rospy.loginfo('The program source file [' + os.getcwd() + '/program.py] generated!') # os.system('cat program.py') if method_name == 'play2': CodeExecution.run_process(['python', 'program.py']) elif method_name == 'play3': # But py3 cannot be used CodeExecution.run_process( ['python3', 'program.py']) else: rospy.logerr('Called unknown method %s', method_name) else: if 'pause' == method_name: CodeStatus.set_current_status(CodeStatus.PAUSED) elif 'resume' == method_name: CodeStatus.set_current_status(CodeStatus.RUNNING) elif 'end' == method_name: #End program.py execution global pid print("@@@@@@@@@@@@@@@@@@") try: print("kill pid=" + str(pid)) os.kill(pid, signal.SIGKILL) ros_nodes = rosnode.get_node_names() except NameError: print("execution script not running.") pass if '/imu_talker' in ros_nodes: #brain ##set default values pub = rospy.Publisher('/statusleds', String, queue_size=10, latch=True) msg = 'blue_off' pub.publish(msg) msg = 'orange_off' pub.publish(msg) if '/crab_leg_kinematics' in ros_nodes: #spider print("spider running") pub = rospy.Publisher('/joy', Joy, queue_size=10, latch=True) msg = Joy() msg.header.stamp = rospy.Time.now() rate = rospy.Rate(10) valueAxe = 0.0 valueButton = 0 for i in range(0, 20): msg.axes.append(valueAxe) for e in range(0, 17): msg.buttons.append(valueButton) pub.publish(msg) print("DEFAULT MESSAGES SENT") if '/mavros' in ros_nodes: #rover print("rover") pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10) msg = OverrideRCIn() msg.channels[0] = 1500 msg.channels[1] = 0 msg.channels[2] = 1500 msg.channels[3] = 0 msg.channels[4] = 0 msg.channels[5] = 0 msg.channels[6] = 0 msg.channels[7] = 0 pub.publish(msg) print("@@@@@@@@@@@@@@@@@@") elif method_name.startswith('control'): robot = method_name.split('control_')[1] if robot.startswith('spider'): direction = robot.split('spider_')[1] pub = rospy.Publisher('/joy', Joy, queue_size=10) msg = Joy() msg.header.stamp = rospy.Time.now() rate = rospy.Rate(10) valueAxe = 0.0 valueButton = 0 for i in range(0, 20): msg.axes.append(valueAxe) for e in range(0, 17): msg.buttons.append(valueButton) if direction == 'up': #forward msg.axes[1] = 1 elif direction == 'down': #backwards msg.axes[1] = -1 elif direction == 'left': #turn left #msg.axes[0] = 1 msg.axes[2] = 1 elif direction == 'right': #turn rigth #msg.axes[0] = -1 msg.axes[2] = -1 pub.publish(msg) rate.sleep() elif robot.startswith('rover'): direction = robot.split('rover_')[1] rospy.wait_for_service('/mavros/set_mode') change_mode = rospy.ServiceProxy( '/mavros/set_mode', SetMode) resp1 = change_mode(custom_mode='manual') print(resp1) if 'True' in str(resp1): pub = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=10) r = rospy.Rate(10) #2hz msg = OverrideRCIn() throttle_channel = 2 steer_channel = 0 speed_slow = 1558 #speed_turbo = 2000 #dangerous speed = speed_slow if direction == 'up': #forward msg.channels[throttle_channel] = speed msg.channels[ steer_channel] = 1385 #straight elif direction == 'down': #backwards msg.channels[ throttle_channel] = 1450 #slow msg.channels[ steer_channel] = 1385 #straight elif direction == 'left': #turn left msg.channels[throttle_channel] = speed msg.channels[steer_channel] = 1285 elif direction == 'right': #turn rigth msg.channels[throttle_channel] = speed msg.channels[steer_channel] = 1485 start = time.time() flag = True while not rospy.is_shutdown() and flag: sample_time = time.time() if ((sample_time - start) > 0.5): flag = False pub.publish(msg) r.sleep() else: rospy.logerr('Called unknown method %s', method_name)
def remove_dead_nodes(self): running_nodes = rosnode.get_node_names() dead_nodes = [node_name for node_name in self.nodes if node_name not in running_nodes] for node_name in dead_nodes: self.nodes.pop(node_name, None)
def main(): rospy.init_node("crane_x7_pick_and_place_controller") robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") arm.set_max_velocity_scaling_factor(0.1) gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) print("Group names:") print(robot.get_group_names()) print("Current state:") print(robot.get_current_state()) # アーム初期ポーズを表示 arm_initial_pose = arm.get_current_pose().pose print("Arm initial pose:") print(arm_initial_pose) # 何かを掴んでいた時のためにハンドを開く gripper.set_joint_value_target([0.9, 0.9]) gripper.go() # SRDFに定義されている"home"の姿勢にする arm.set_named_target("home") arm.go() gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # 掴む準備をする target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.0 target_pose.position.z = 0.3 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # ハンドを開く gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # 掴みに行く target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.0 target_pose.position.z = 0.13 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # ハンドを閉じる gripper.set_joint_value_target([0.4, 0.4]) gripper.go() # 持ち上げる target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.0 target_pose.position.z = 0.3 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # 移動する target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.2 target_pose.position.z = 0.3 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # 下ろす target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.2 target_pose.position.z = 0.13 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # ハンドを開く gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # 少しだけハンドを持ち上げる target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.2 target_pose.position.z = 0.2 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # SRDFに定義されている"home"の姿勢にする arm.set_named_target("home") arm.go() print("done")
def do_dump(self, line): """Dump information""" nodes = rosnode.get_node_names() rospy.loginfo("node list:%s" % (nodes))
def do_test(self, line): """Current test function""" nodes = rosnode.get_node_names() rospy.loginfo("node list:%s" % (nodes))
def __init__(self): self.ekf_pub = rospy.Publisher('/ekf', Odometry, queue_size=10) #%% Encoder variables self.flag_encR = True self.flag_encL = True self.wl_ini = 0.0 self.wr_ini = 0.0 self.radio = 0.0725 self.enc_Ts = 0.0 self.enc_tic = 0.0 self.wr = 0.0 self.wl = 0.0 self.wr_1 = 0.0 self.wl_1 = 0.0 self.wr_2 = 0.0 self.wl_2 = 0.0 #%% Filter_encoders variables self.acce = 20.0 self.wr_f_2 = 0.0 self.wr_f_1 = 0.0 self.wl_f_2 = 0.0 self.wl_f_1 = 0.0 #%% ICR estimation self.ICR = 0.0 self.ICR_1 = 0.0 self.ICR_2 = 0.0 self.ICR_f1 = 0.0 self.ICR_f2 = 0.0 #%% IMU variables self.flag_imu = True self.euler1 = [0, 0, 0] self.imu_toc = 0.0 self.imu_Ts = 0.0 self.imu_tic = 0.0 self.imu_pitch = 0.0 self.imu_roll = 0.0 self.imu_yaw = 0.0 #%% Mag variables self.flag_mag = True self.yaw_mag1 = 0.0 self.mag_toc = 0.0 self.mag_Ts = 0.0 self.mag_tic = 0.0 self.mag_yaw = 0.0 self.mag_yaw_1 = 0.0 self.Wz = 0.0 self.x_mag = 0.0 self.y_mag = 0.0 self.z_mag = 0.0 self.Wz_2 = 0.0 self.Wz_1 = 0.0 self.Wz_f2 = 0.0 self.Wz_f1 = 0.0 #%% Kalman variables # Matrix error of covariance self.kalman_P = np.matrix([[100.0, 0.0, 0.0], [0.0, 100.0, 0.0], [0.0, 0.0, 100.0]]) # Covariance matrix of process noise # self.kalman_Q = np.matrix([[1, 0.0, 0.0],[0.0, 1, 0.0],[0.0, 0.0, 0.001]]) self.kalman_Q = np.matrix([[ 5.627197978234252e-07, -8.660344555066894e-08, 1.905427279718744e-06 ], [ -8.660344555066894e-08, 1.4065639626880477e-08, -2.933464617904681e-07 ], [ 1.905427279718744e-06, -2.933464617904681e-07, 6.460098912945566e-06 ]]) # Covariance matrix of measurement noise self.kalman_R = 0.0001817647107593486 self.Yg = 1.2 self.X_hat = np.matrix([[0.0], [0.0], [0.0]]) #%% INIT self.today = date.today() self.time = rospy.Time.now() print("Starting Kalman node - V2.5.T") rospy.Subscriber('/WR', Float32, self.Wr_callbaback) rospy.Subscriber('/WL', Float32, self.Wl_callbaback) rospy.Subscriber('/imu/data', Imu, self.calIMU) #100 Hz rospy.Subscriber('/imu/mag', MagneticField, self.calMag) #100 Hz Flag_out = False self.Flag_start = False while not Flag_out: node_names = rosnode.get_node_names() # bag_node = [i for i in node_names if 'play' in i] bag_node = [i for i in node_names if 'xsens' in i] if (len(bag_node) == 0) and self.Flag_start: rospy.signal_shutdown('finish') Flag_out = True else: self.Kalman() exit()
def onMessage(self, payload, isBinary): # Debug if isBinary: print("Binary message received: {0} bytes".format(len(payload))) else: print("Text message received: {0}".format(payload.decode('utf8'))) # Do stuff # pub = rospy.Publisher('blockly', String, queue_size=10) # time.sleep(1) # pub.publish("blockly says: "+payload.decode('utf8')) # Simple text protocol for communication # first line is the name of the method # next lines are body of message message_text = payload.decode('utf8') message_data = message_text.split('\n', 1) if len(message_data) > 0: method_name = message_data[0] if len(message_data) > 1: method_body = message_data[1] if 'play' == method_name: CodeStatus.set_current_status(CodeStatus.RUNNING) BlocklyServerProtocol.build_ros_node(method_body) rospy.loginfo('The file generated contains...') os.system('cat test.py') CodeExecution.run_process(['python3', 'test.py']) else: rospy.logerr('Called unknown method %s', method_name) else: if 'pause' == method_name: CodeStatus.set_current_status(CodeStatus.PAUSED) elif 'resume' == method_name: CodeStatus.set_current_status(CodeStatus.RUNNING) elif 'end' == method_name: #End test.py execution global pid print("@@@@@@@@@@@@@@@@@@") try: print("kill pid="+str(pid)) os.kill(pid, signal.SIGKILL) ros_nodes = rosnode.get_node_names() if '/imu_talker' in ros_nodes: #brain ##set default values pub = rospy.Publisher('/statusleds', String, queue_size=10, latch=True) msg = 'blue_off' pub.publish(msg) msg = 'orange_off' pub.publish(msg) if '/crab_leg_kinematics' in ros_nodes: #spider print("spider running") pub = rospy.Publisher('/joy', Joy, queue_size=10, latch=True) msg = Joy() msg.header.stamp = rospy.Time.now() rate = rospy.Rate(10) valueAxe = 0.0 valueButton = 0 for i in range (0, 20): msg.axes.append(valueAxe) for e in range (0, 17): msg.buttons.append(valueButton) pub.publish(msg) rate.sleep() print("DEFAULT MESSAGES SENT") print("@@@@@@@@@@@@@@@@@@") except NameError: print("execution script not running.") pass else: rospy.logerr('Called unknown method %s', method_name)
if __name__ == '__main__': print('[INFO] Starting ros communication system...') script_path = os.path.dirname(os.path.realpath(__file__)) uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch_pcd = roslaunch.parent.ROSLaunchParent(uuid, [os.path.join(script_path, '..', 'launch', 'grasping_pcd_processing.launch')]) launch_pcd.start() rospy.sleep(5) capture_helper = CaptureHelper() ROSSMartServo_on = False while not ROSSMartServo_on: rospy.loginfo('Please make sure you have started the ROSSMartServo application on the Sunrise Cabinet') ans = raw_input( '[USER INPUT] Type [y] and press [enter] if you have started the ROSSMartServo, otherwise exit the program: ') if ans == 'y': if '/iiwa/iiwa_subscriber' in rosnode.get_node_names(): ROSSMartServo_on = True else: rospy.loginfo('IIWA topics not detected, check network connection if you have started the SmartServo') else: rospy.loginfo('Exiting the program...') launch_pcd.shutdown() exit() print('[INFO] Starting kuka controller...') launch_controller = roslaunch.parent.ROSLaunchParent(uuid, [os.path.join(script_path, '..', 'launch', 'grasping_controller.launch')]) launch_controller.start() rospy.sleep(5) rospy.loginfo('Ros communication system has been properly setup') while True:
ang = q.x #print pos.x, self.home_x #print "X ",q.x, " Y ", q.y, " Z ", q.z, " W ", q.w br.sendTransform(( pos.x-self.home_x, pos.y-self.home_y, pos.z), (q.x, q.y, q.z, q.w), rospy.Time.now(), self.copter_name, "fcu") if __name__ == '__main__': rospy.init_node('px4_swarm_tfs_broadcaster') br = tf.TransformBroadcaster() rate = rospy.Rate(10.0) for copter_name in rosnode.get_node_names(): if "mavros" in copter_name: pb = publish_copter_frame(copter_name) rospy.Subscriber(copter_name+'/global_position/local', Odometry, pb.handle_pose) while not rospy.is_shutdown(): try: pass except KeyboardInterrupt: rospy.signal_shutdown("Keyboard Interrupt") rate.sleep() rospy.spin()
def test_node_starts(self): nodes = rosnode.get_node_names() self.assertIn(node_name, nodes)
def get_nodes(): """ Returns a list of all the nodes registered in the ROS system """ return get_node_names()
def update_nodepkgmap(self): master = roslib.scriptutil.get_master() nodes = rosnode.get_node_names() for n in nodes: self.nodepkgmap[n] = self.getnodepkg1(n)[0]
def main(): rospy.init_node("crane_x7_pick_and_place_controller") robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") arm.set_max_velocity_scaling_factor(0.1) gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) #print("Group names:") #print(robot.get_group_names()) #print("Current state:") # print(robot.get_current_state()) # アーム初期ポーズを表示 arm_initial_pose = arm.get_current_pose().pose #print("Arm initial pose:") #print(arm_initial_pose) # 何かを掴んでいた時のためにハンドを開く gripper.set_joint_value_target([0.9, 0.9]) gripper.go() # SRDFに定義されている"home"の姿勢にする arm.set_named_target("home") arm.go() gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # 掴む準備をする target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0 target_pose.position.y = -0.3 target_pose.position.z = 0.3 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # ハンドを開く gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # 掴みに行く target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0 target_pose.position.y = -0.3 target_pose.position.z = 0.07 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # ハンドを閉じる gripper.set_joint_value_target([0.2, 0.2]) gripper.go() # SRDFに定義されている"landing"の姿勢にする arm.set_named_target("landing") arm.go() # ハンドを開く gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # SRDFに定義されている"vertical"の姿勢にする #アルコールに当たることがあるので一度アームを垂直にする。 arm.set_named_target("vertical") arm.go() #アームを開いて押す準備をする gripper.set_joint_value_target([0.7, 0.7]) gripper.go() #アームをアルコールの上に持ってくる target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.1 target_pose.position.z = 0.33 q = quaternion_from_euler(-3.14, 0.0, -0.7) #手首をz軸に80度ほど回転させた姿勢にしてボトルのノズルを押しやすくする。 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 gripper.set_joint_value_target([0.7, 0.7]) gripper.go() #アルコールボトルを押す target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.1 target_pose.position.z = 0.21 q = quaternion_from_euler(-3.14, 0, -0.7) #先程の手首をz軸に80度ほど回転させた姿勢にする。 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 rospy.sleep(1.0) #アルコールボトルからアームを引く target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.1 target_pose.position.z = 0.33 q = quaternion_from_euler(-3.14, 0.0, -0.7) #先程の手首をz軸に80度ほど回転させた姿勢にする。 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 #home(元々の)の姿勢に戻す。 arm.set_named_target("home") arm.go() print("done")
def test_node_exist(self): nodes = rosnode.get_node_names() self.assertIn('/buzzer',nodes, "node does not exist")
def main(): rospy.init_node("crane_x7_pick_and_place_controller") robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") def move_max_velocity(value=0.5): arm.set_max_velocity_scaling_factor(value) gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) print("Group names:") print(robot.get_group_names()) print("Current state:") print(robot.get_current_state()) ### ### b-group ### #紙Aコップのx座標 紙Aコップのy座標 紙Bコップx座標 紙Bコップy座標 position_base = [[0.29, 0.13, 0.29, -0.11]] Acup_tukamu = False Bcup_tukamu = False """ 使い方 ハンドをつかんだ時は position_manager(False,False,x,y,True) ハンドをはなした時は position_manager(False,False,x,y,False) Aカップをつかみたい時の手先の位置を知りたいときは aa = position_manager(True,True,0,0,False) x = aa[0] y = aa[1] BBカップをつかみたい時の手先の位置を知りたいときは aa = position_manager(True,False,0,0,False) x = aa[0] y = aa[1] """ def position_manager(master_judge, paper_cup, x, y, tukami): global Acup_tukamu global Bcup_tukamu position_ret = [0.0, 0.0] if master_judge == True: if len(position_base) > 0: #ホンスワン if paper_cup == True: position_ret[0] = position_base[len(position_base)-1][2] position_ret[1] = position_base[len(position_base)-1][3] #position_historyの末尾の紙Aコップのx座標y座標 return position_ret else: position_ret[0] = position_base[len(position_base)-1][0] position_ret[1] = position_base[len(position_base)-1][1] #position_historyの末尾の紙Bコップのx座標y座標 return position_ret else: return position_ret else: #熊谷さん #position_base配列の末尾にx,yを追加 if tukami == True: #Aをつかんでいる場合 if position_base[len(position_base)-1][0] == x and position_base[len(position_base)-1][1] == y: position_base.append([x, y, position_base[len( position_base)-1][2], position_base[len(position_base)-1][3]]) Acup_tukamu = True #Bをつかんでいる場合 elif position_base[len(position_base)-1][2] == x and position_base[len(position_base)-1][3] == y: position_base.append([position_base[len( position_base)-1][0], position_base[len(position_base)-1][1], x, y]) Bcup_tukamu = True else: #Aをはなした時 if Acup_tukamu == True: position_base.append([x, y, position_base[len( position_base)-1][2], position_base[len(position_base)-1][3]]) Acup_tukamu = False elif Bcup_tukamu == True: position_base.append([position_base[len( position_base)-1][0], position_base[len(position_base)-1][1], x, y]) Bcup_tukamu = False return position_ret ### ### b-group-end ### ### ### a-group ### stop_time = 2.0 # 停止する時間を指定 weak_position = 0.13 # 上をつかむ際のz weak_power = 0.24 # 上をつかむ際の力 strong_position = 0.11 # 下をつかむ際のz strong_power = 0.28 # 下をつかむ際の力 # アーム初期ポーズを表示 arm_initial_pose = arm.get_current_pose().pose print("Arm initial pose:") print(arm_initial_pose) # ハンドを開く/ 閉じる def move_gripper(pou): gripper.set_joint_value_target([pou, pou]) gripper.go() # アームを移動する def move_arm(pos_x, pos_y, pos_z): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x target_pose.position.y = pos_y target_pose.position.z = pos_z q = quaternion_from_euler(-3.14/2.0, 3.14, -3.14/2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # コップ上部をつかむ位置へ移動1 def move_arm_upper(pos_x, pos_y): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x target_pose.position.y = pos_y target_pose.position.z = 0.25 q = quaternion_from_euler(0, 3.14, 0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # コップ上部をつかむ位置へ移動2 def move_arm_upper_catch(pos_x, pos_y): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x target_pose.position.y = pos_y target_pose.position.z = weak_position q = quaternion_from_euler(0, 3.14, 0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # コップ下部をつかむ位置へ移動1 def move_arm_lower(pos_x, pos_y): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x target_pose.position.y = pos_y target_pose.position.z = 0.25 q = quaternion_from_euler(0, 3.14, 0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # コップ下部をつかむ位置へ移動2 def move_arm_lower_catch(pos_x, pos_y): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x target_pose.position.y = pos_y target_pose.position.z = strong_position q = quaternion_from_euler(0, 3.14, 0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # コップの上を持ち上へ移動 def move_arm_upper_up(pos_x, pos_y): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x target_pose.position.y = pos_y target_pose.position.z = 0.25 q = quaternion_from_euler(0, 3.14, 0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # コップの下を持ち上へ移動 def move_arm_lower_up(pos_x, pos_y): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x target_pose.position.y = pos_y target_pose.position.z = 0.25 q = quaternion_from_euler(0, 3.14, 0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # ボールをつかむ位置へ移動 def move_arm_ball(pos_x, pos_y): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x target_pose.position.y = pos_y target_pose.position.z = 0.1 q = quaternion_from_euler(0, 3.14, 0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 ### ### a-group-end ### # SRDFに定義されている"home"の姿勢にする arm.set_named_target("home") arm.go() #ハンドを開く move_gripper(1.3) #コップA-----1 #aaにコップAの(x,y)を代入 aa = position_manager(True, True, 0, 0, False) #グリッパを開く move_gripper(1.3) #アームをコップをくっつける move_arm_upper_catch(aa[0], aa[1]) print("グリッパ下にボールを入れた紙コップを設置してください。") rospy.sleep(stop_time) print("次の動作へ進みます。") #アームを持ち上げコップから離れたらホームへ戻る move_arm_lower_up(aa[0], aa[1]) move_max_velocity() arm.set_named_target("home") arm.go() #コップB-----2 #aaにコップBの(x,y)を代入 aa = position_manager(True, False, 0, 0, False) #グリッパを開く move_gripper(1.3) #アームをコップの下部とくっつける move_arm_upper_catch(aa[0], aa[1]) print("グリッパ下に空の紙コップを設置してください。") rospy.sleep(stop_time) print("次の動作へ進みます。") #アームを持ち上げコップから離れたらホームへ戻る move_arm_upper_up(aa[0], aa[1]) move_max_velocity() arm.set_named_target("home") arm.go() #ボール-----3 #aaにコップBの(x,y)を代入 aa = position_manager(True, False, 0, 0, False) #グリッパを開く move_gripper(1.3) #アームをボールとくっつける move_arm_ball(aa[0], aa[1] + 0.1) print("グリッパ下にボールを設置してください。") rospy.sleep(stop_time) print("次の動作へ進みます。") #アームを持ち上げコップから離れたらホームへ戻る move_arm_ball(aa[0], aa[1] + 0.1) move_max_velocity() arm.set_named_target("home") arm.go()
service_name = rospy.get_param('~service') tries = 0 try: service = rospy.ServiceProxy(service_name, std_srvs.srv.Empty) rospy.loginfo("Waiting for service %s", service_name) service.wait_for_service() rospy.loginfo("Starting service, will wait for %f seconds", wait_time) rate = rospy.Rate(spin_rate) while not rospy.is_shutdown(): #rospy.Time.now() < end: if trigger in rosnode.get_node_names(): rospy.loginfo("waiting for %f seconds now that %s is up", wait_time, trigger) rospy.sleep(wait_time) rospy.loginfo("trying to publish scene information...") service() tries = tries + 1 if tries > 5: break rate.sleep() except rospy.ROSInterruptException: pass
def drone_viz(): rospy.init_node("viz") drones = [] position_dict = {} waypoints_dict = {} waypointsbox_dict = {} pub_dict = {} rate = rospy.Rate(10) pub = rospy.Publisher("viz/drone_markers", MarkerArray, queue_size=1) markers = {} waypoints = {} waypointsbox = {} trajectories = {} marker_array = MarkerArray() global_id = 0 BOX = "box" while not rospy.is_shutdown(): #on fixed timestip: simulate the system and publish nodes = rosnode.get_node_names() for node in nodes: node = node[1:] if node.startswith("drone") and node not in drones or node == BOX: drones.append(node) rospy.Subscriber("/" + node + "/position", Pose, callback, (node, position_dict)) rospy.Subscriber("/" + node + "/waypoint", Float32MultiArray, callback, (node, waypoints_dict)) t, lifetime = rospy.Time.now(), rospy.Duration() if BOX in drones and BOX in position_dict: if BOX not in markers: markers[BOX] = Marker() marker = markers[BOX] marker.header.frame_id = "world" marker.id = global_id global_id += 1 marker.header.stamp = t marker.type = Marker.CUBE marker.action = Marker.ADD marker.scale = Vector3(0.1,0.1,0.1) marker.color.r = 0.4 marker.color.g = 1 marker.color.b = 0.4 marker.color.a = 1.0 marker.lifetime = lifetime marker_array.markers.append(marker) marker = markers[BOX] marker.pose = position_dict[BOX] if BOX not in trajectories: trajectories[BOX] = Marker() marker = trajectories[BOX] marker.header.frame_id = "world" marker.id = global_id global_id += 1 marker.header.stamp = t marker.type = Marker.LINE_STRIP marker.action = Marker.ADD marker.scale = Vector3(0.01,0.01,0.01) marker.color.r = 0.4 marker.color.g = 1 marker.color.b = 0.7 marker.color.a = 1.0 marker.lifetime = lifetime marker_array.markers.append(marker) marker = trajectories[BOX] p = Point() p.x = position_dict[BOX].position.x p.y = position_dict[BOX].position.y p.z = position_dict[BOX].position.z marker.points.append(p) # marker.points = marker.points[-400:] for i, drone in enumerate(drones): if drone == BOX: continue if drone not in position_dict: continue if drone not in markers: markers[drone] = Marker() marker = markers[drone] marker.header.frame_id = "world" marker.id = global_id global_id += 1 marker.header.stamp = t marker.type = Marker.MESH_RESOURCE marker.mesh_resource = "package://drone_viz/src/drone.stl" #marker.type = Marker.CYLINDER #marker.action = Marker.ADD marker.scale = Vector3(0.001,0.001,0.001) marker.color.r = 0.4 marker.color.g = 1 marker.color.b = 1 marker.color.a = 1.0 marker.lifetime = lifetime marker_array.markers.append(marker) marker = markers[drone] marker.pose = position_dict[drone] marker.pose = Pose() marker.pose.position.x = position_dict[drone].position.x marker.pose.position.y = position_dict[drone].position.y - 0.04 marker.pose.position.z = position_dict[drone].position.z pos_msg = position_dict[drone] euler = np.array(euler_from_quaternion([ pos_msg.orientation.x, pos_msg.orientation.y, pos_msg.orientation.z, pos_msg.orientation.w ])) euler[0] += -np.pi/2 quat = quaternion_from_euler(euler[0], euler[1], euler[2]) marker.pose.orientation.x = quat[0] marker.pose.orientation.y = quat[1] marker.pose.orientation.z = quat[2] marker.pose.orientation.w = quat[3] #waypoint if drone not in waypoints_dict: continue if drone not in waypoints: waypoints[drone] = Marker() marker = waypoints[drone] marker.header.frame_id = "world" marker.id = global_id global_id += 1 marker.header.stamp = t marker.type = Marker.POINTS marker.action = Marker.ADD marker.scale = Vector3(0.05,0.05,0.05) marker.color.r = 1 marker.color.g = 0.4 marker.color.b = 0.4 marker.color.a = 1.0 marker.lifetime = lifetime marker_array.markers.append(marker) marker = waypoints[drone] if len(marker.points) == 0 or marker.points[-1] != waypoints_dict[drone]: p = Point() p.x = waypoints_dict[drone].data[0] p.y = waypoints_dict[drone].data[1] - 0.04 p.z = waypoints_dict[drone].data[2] marker.points.append(p) #trajectories if drone not in trajectories: trajectories[drone] = Marker() marker = trajectories[drone] marker.header.frame_id = "world" marker.id = global_id global_id += 1 marker.header.stamp = t marker.type = Marker.LINE_STRIP marker.action = Marker.ADD marker.scale = Vector3(0.01,0.01,0.01) marker.color.r = 0.4 marker.color.g = 0.7 marker.color.b = 1 marker.color.a = 1.0 marker.lifetime = lifetime marker_array.markers.append(marker) marker = trajectories[drone] p = Point() p.x = position_dict[drone].position.x p.y = position_dict[drone].position.y - 0.04 p.z = position_dict[drone].position.z marker.points.append(p) # marker.points = marker.points[-400:] if BOX in drones and BOX in position_dict: # draw rope marker_name = "{}-{}".format(BOX, drone) if marker_name not in markers: markers[marker_name] = Marker() marker = markers[marker_name] marker.header.frame_id = "world" marker.id = global_id global_id += 1 marker.header.stamp = t marker.type = Marker.LINE_STRIP marker.action = Marker.ADD marker.scale = Vector3(0.008,0.008,0.008) marker.color.r = 1 marker.color.g = 0.7 marker.color.b = 0.4 marker.color.a = 1.0 marker.lifetime = lifetime marker_array.markers.append(marker) marker = markers[marker_name] drone_pos = position_dict[drone] drone_point = Point(drone_pos.position.x, drone_pos.position.y, drone_pos.position.z - 0.01) box_pos = position_dict[BOX] box_point = Point(box_pos.position.x, box_pos.position.y, box_pos.position.z) marker.points = [drone_point, box_point] pub.publish(marker_array) rate.sleep()
def cleanup(self, node_name): master = rosgraph.Master(rosnode.ID) nodes_to_cleanup = [n for n in rosnode.get_node_names() if n.startswith(node_name)] rosnode.cleanup_master_blacklist(master, nodes_to_cleanup)
def test_running(self): nodes = rosnode.get_node_names() self.assertIn('/gazebo', nodes, 'node does not exit') self.assertIn('/gmapping', nodes, 'node does not exit')
def main(): rospy.init_node("terminal_control") robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") arm.set_max_velocity_scaling_factor(0.1) gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) print("Group names:") print(robot.get_group_names()) print("Current state:") print(robot.get_current_state()) # アーム初期ポーズを表示 arm_initial_pose = arm.get_current_pose().pose print("Arm initial pose:") print(arm_initial_pose) # 何かを掴んでいた時のためにハンドを開く gripper.set_joint_value_target([0.9, 0.9]) gripper.go() # SRDFに定義されている"home"の姿勢にする arm.set_named_target("home") arm.go() gripper.set_joint_value_target([0.7, 0.7]) gripper.go() x_pos = 0.2 y_pos = 0.0 z_pos = 0.3 x_ang = -3.14 y_ang = 0.0 z_ang = -3.14 / 2.0 grip_ang = 0.7 while (True): print("Arm current pose:") print(arm.get_current_pose().pose) input_mode = raw_input( "Position (p), Orientation (o) or both (b)? Toggle Gripper (t). Otherwise, quit (q) " ) if (input_mode is not "p" and input_mode is not "o" and input_mode is not "b" and input_mode is not "q" and input_mode is not "t"): print("Try again") continue elif input_mode is "q": print("Exiting command mode, shutting down...") break if input_mode is "p" or input_mode is "b": while (True): #keep trying until get 3 valid points try: x_pos = float( raw_input( "Cartesian Coordinate (x) in metres from base: ")) y_pos = float( raw_input( "Cartesian Coordinate (y) in metres from base: ")) z_pos = float( raw_input( "Cartesian Coordinate (z) in metres from base: ")) break except: print("Enter valid float values, please") pass if input_mode is "o" or input_mode is "b": while (True): #keep trying until get 3 valid points try: x_ang = float(raw_input("Angle about x-axis in radians: ")) y_ang = float(raw_input("Angle about y-axis in radians: ")) z_ang = float(raw_input("Angle about z-axis in radians: ")) break except: print("Enter valid float values, please") pass if (input_mode is not "t"): # 掴む準備をする target_pose = geometry_msgs.msg.Pose() target_pose.position.x = x_pos #0.2 target_pose.position.y = y_pos #0.0 target_pose.position.z = z_pos #0.3 q = quaternion_from_euler( x_ang, y_ang, z_ang) #(-3.14, 0.0, -3.14/2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 elif input_mode is "t": # ハンドを開く grip_ang = 0.7 if grip_ang < 0.7 else 0.3 gripper.set_joint_value_target([grip_ang, grip_ang]) #([0.7, 0.7]) gripper.go() print("done") print("shutting down the arm...")
#!/usr/bin/env python from reflex_gripper.reflex_sf_hand import ReflexSFHand from std_srvs.srv import Empty import argparse, sys, rosnode, rospy if __name__ == "__main__": parser = argparse.ArgumentParser( description="Calibrate a Gripper Hand") parser.add_argument('name', help="The name of the gripper hand to calibrate") args = parser.parse_args(sys.argv[1:]) name = args.name if '/reflex_%s'%name in rosnode.get_node_names(): print "If you do not see the script running in this window, " print "please check the window that the hand service is running in." rospy.wait_for_service('reflex_%s/zero_fingers'%name, 5) service = rospy.ServiceProxy('/reflex_%s/zero_fingers'%name, Empty) service() else: hand = ReflexSFHand(name) hand.calibrate()
def main(): rospy.init_node("crane_x7_pick_and_place_controller") robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") def move_max_velocity(value=0.5): arm.set_max_velocity_scaling_factor(value) gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) print("Group names:") print(robot.get_group_names()) print("Current state:") print(robot.get_current_state()) stop_time = 2.0 # 停止する時間を指定 force_hold_stick = 0.5 # 棒を握る力を指定 te_x_position_vertical = 0.193040 # x te_y_position_vertical = -0.233386 # y te_z_position_vertical = 0.085469 # z stick_angle_vertical = 1.3 # 棒 # アーム初期ポーズを表示 arm_initial_pose = arm.get_current_pose().pose print("Arm initial pose:") print(arm_initial_pose) # ハンドを開く/ 閉じる def move_gripper(pou): gripper.set_joint_value_target([pou, pou]) gripper.go() # アームを移動する def move_arm(pos_x, pos_y, pos_z): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x target_pose.position.y = pos_y target_pose.position.z = pos_z q = quaternion_from_euler(-3.14 / 2.5, 3.14, -3.14 / 2.0) target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) arm.go() def preparation_vertical(): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = te_x_position_vertical target_pose.position.y = te_y_position_vertical target_pose.position.z = te_z_position_vertical q = quaternion_from_euler(-3.14 / 2.0, 3.14, -3.14 / 2.0) target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) arm.go() def preparation2_vertical(): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = te_x_position_vertical target_pose.position.y = te_y_position_vertical target_pose.position.z = te_z_position_vertical q = quaternion_from_euler(-3.14 / 2.15, 3.14, -3.14 / 2.0) target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) arm.go() def p1_vertical(): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = te_x_position_vertical target_pose.position.y = te_y_position_vertical target_pose.position.z = te_z_position_vertical q = quaternion_from_euler(0, 3.14, -0) target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) arm.go() def p2_vertical(): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = te_x_position_vertical target_pose.position.y = te_y_position_vertical target_pose.position.z = te_z_position_vertical q = quaternion_from_euler(0, 3.14, -3.14 / 2.0) target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) arm.go() def p3_vertical(): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = te_x_position_vertical target_pose.position.y = te_y_position_vertical target_pose.position.z = te_z_position_vertical q = quaternion_from_euler(1, 3.14, 0) target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) arm.go() def p4_vertical(): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = te_x_position_vertical target_pose.position.y = te_y_position_vertical target_pose.position.z = te_z_position_vertical q = quaternion_from_euler(0, 3.14, -3.14 / 2.0) target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) arm.go() preparation_vertical() preparation_vertical() p1_vertical() preparation2_vertical() p2_vertical() preparation2_vertical() move_max_velocity() arm.set_named_target("home") arm.go()
def commandCB(self, data): if self.status == NOT_FLYING: self.goal.yaw = utils.quat2yaw(self.pose.orientation) if data.command == data.CMD_TAKEOFF and self.status == NOT_FLYING: if '/rosbag_ft' in rosnode.get_node_names(): self.status = FLYING self.wpType = TAKEOFF # set initial goal to current pose self.goal.pos = copy.copy(self.pose.position) self.goal.vel.x = self.goal.vel.y = self.goal.vel.z = 0 self.goal.yaw = utils.quat2yaw(self.pose.orientation) self.goal.dyaw = 0 rospy.loginfo("Flying") else: rospy.logerr('Can\'t takeoff without logging !! --Nick Roy') elif data.command == data.CMD_HOVER and self.go == True: rospy.loginfo("Entering state of hover") self.stop_now = True # emergency disable elif data.command == data.CMD_KILL and self.status == FLYING: self.status = NOT_FLYING self.wpType = DISABLE self.go = False # landing elif data.command == data.CMD_LAND and self.status == FLYING: self.go = False self.status = LANDING # self.wpType = LAND self.goal.pos.x = self.pose.position.x self.goal.pos.y = self.pose.position.y self.goal.vel.x = 0 self.goal.vel.y = 0 self.goal.vel.z = 0 self.goal.dyaw = 0 self.I = 0 elif data.command == data.CMD_GO and self.status == FLYING: if self.received_coeff: self.eval_splines() self.goal.pos.x = self.x self.goal.pos.y = self.y self.goal.pos.z = self.z self.goal.vel.x = 0 self.goal.vel.y = 0 self.goal.vel.z = 0 self.goal.yaw = 0 #0*np.arctan2(self.T[1],self.T[0]) self.dyaw = 0 rospy.loginfo("Ready") else: rospy.loginfo("Haven't received coefficients.") elif data.command == data.CMD_INIT and self.status == FLYING: if self.received_coeff: rospy.loginfo("Starting") self.go = True else: rospy.loginfo("Haven't received coefficients.") if self.status == TAKEOFF: self.goal.pos.z = utils.saturate(self.goal.pos.z+0.0025, self.alt,-0.1) if self.status == LANDING: if self.pose.position.z > 0.4: # fast landing self.goal.pos.z = utils.saturate(self.goal.pos.z - 0.0035, 2.0, -0.1) else: # slow landing self.goal.pos.z = utils.saturate(self.goal.pos.z - 0.001, 2.0, -0.1) if self.goal.pos.z == -0.1: self.status = NOT_FLYING self.wpType = DISABLE rospy.loginfo("Landed!")
def test_node_exist(self): nodes = rosnode.get_node_names() self.assertIn('/referee_receiver', nodes, 'node does not exist')
import rospy import math import time import rosnode # Import message file from std_msgs.msg import String from khepera_communicator.msg import K4_controls, SensorReadings from geometry_msgs.msg import TransformStamped start = time.time() rospy.init_node('Central_Algorithm', anonymous=True) # Get all the node names for all the currently running K4_Send_Cmd nodes (all running Kheperas) # Get the node names of all the current running nodes node_list = rosnode.get_node_names() # Find the nodes that contains the "K4_Send_Cmd_" title khep_node_list = [s for s in node_list if "K4_Send_Cmd_" in s] ip_num_list = [x[13:16] for x in khep_node_list] khep_node_cnt = len(khep_node_list) # Establish all the publishers to each "K4_controls_" topic, corresponding to each K4_Send_Cmd node, which corresponds to each Khepera robot pub = [] for i in range(khep_node_cnt): pub.append( rospy.Publisher('K4_controls_' + str(ip_num_list[i]), K4_controls, queue_size=10))
import rospy import rosnode import math from tf.transformations import euler_from_quaternion from nav_msgs.msg import Odometry from geometry_msgs.msg import PointStamped, PoseStamped, Quaternion #check if slam get started ros_nodes = rosnode.get_node_names() if ('/gmapping' not in ros_nodes) or ('/move_base' not in ros_nodes): print('SLAM not started') else: print('Move in SLAM mode') rospy.init_node('xbot_move_slam') odom_data = rospy.wait_for_message('/odom', Odometry, timeout=5) # TODO: # get x_pos, y_pos, and quaternion of new goal # x_pos = # y_pos = # yaw = #create goal goal = PoseStamped() goal.header = odom_data.header #cal goal_x, goal_y goal.pose.position.x = x_pos goal.pose.position.y = y_pos goal.pose.position.z = 0.0 #goal.pose.orientation = euler_to_quaternion()
def _list(self): """ Override Namespace._list() """ return rosnode.get_node_names(namespace=self._ns)
def main(): # rospy.init_node("pose_groupstate_example") robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") arm.set_max_velocity_scaling_factor(1.0) gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) print("Group names:") print(robot.get_group_names()) print("Current state:") print(robot.get_current_state()) # アーム初期ポーズを表示 arm_initial_pose = arm.get_current_pose().pose print("Arm initial pose:") print(arm_initial_pose) print("vertical") arm.set_named_target("vertical") arm.go() print("vertical1") arm.set_named_target("vertical1") arm.go() print("vertical") arm.set_named_target("vertical") arm.go() print("vertical1") arm.set_named_target("vertical1") arm.go() print("vertical") arm.set_named_target("vertical") arm.go() print("home") arm.set_named_target("home") arm.go() print("vertical") arm.set_named_target("vertical") arm.go() print("vertical1") arm.set_named_target("vertical1") arm.go() print("vertical") arm.set_named_target("vertical") arm.go() print("vertical1") arm.set_named_target("vertical1") arm.go() print("vertical") arm.set_named_target("vertical") arm.go() print("home") arm.set_named_target("home") arm.go() arm_goal_pose = arm.get_current_pose().pose print("Arm goal pose:") print(arm_goal_pose) print("done")
import sys import rospy import subprocess import rosnode import numpy as np import cv2 import time import os import rospkg timestr = time.strftime("%d-%m-%Y_%H-%M-%S.png") # Ros Messages from sensor_msgs.msg import CompressedImage ros_nodes = rosnode.get_node_names() if '/raspicam_node' in ros_nodes: command='rosservice call /camera/start_capture' process = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE) else: command='/home/erle/ros_catkin_ws/install_isolated/camera.sh' command+=';rosservice call /camera/start_capture' process = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE) msg_image = rospy.wait_for_message('/camera/image/compressed', CompressedImage, timeout=7) np_arr = np.fromstring(msg_image.data, np.uint8) image_np = cv2.imdecode(np_arr, 1) #cv2.CV_LOAD_IMAGE_COLOR rospack = rospkg.RosPack() images_path = rospack.get_path('robot_blockly') + '/frontend/pages/images/' cv2.imwrite(images_path+ 'image_' + timestr, image_np)
#!/usr/bin/env python from reflex_gripper.reflex_sf_hand import ReflexSFHand from std_srvs.srv import Empty import argparse, sys, rosnode, rospy if __name__ == "__main__": parser = argparse.ArgumentParser( description="Calibrate a Gripper Hand with the interactive script") parser.add_argument('name', help="The name of the gripper hand to calibrate") args = parser.parse_args(sys.argv[1:]) name = args.name if '/reflex_%s'%name in rosnode.get_node_names(): print "If you do not see the script running in this window, " print "please check the window that the hand service is running in." rospy.wait_for_service('reflex_%s/zero_fingersI'%name, 5) service = rospy.ServiceProxy('/reflex_%s/zero_fingersI'%name, Empty) service() else: hand = ReflexSFHand(name) hand.calibrateI()
for i in range(0,len(names)): full_command += ['--tab', '-e', "bash -c \"roslaunch reflex_gripper reflex_gripper.launch " +\ "Name:=%s Port:=%s\"" %(names[i],ports[i])] subprocess.call(['gnome-terminal'] + full_command) print "--------------------------------------------" # Make sure the required nodes have successfully launched print "Checking for successful launch of all required nodes..." time.sleep(4) found = False for name in names: # check that finger was successfully started print "\tChecking %s..." % name found = "/dynamixel_manager_%s" % name in rosnode.get_node_names() if not bkgOnly: found = found and "/reflex_%s" % name in rosnode.get_node_names() if(not found): print "%s: error: It appears that the required " %(progName) +\ "nodes were not successfully started. Please try again." return -1 print "--------------------------------------------" # if RRService specified try starting the RR Service if RRService: try: RRpkg = rospack.get_path('reflex_gripper_rr_bridge') print "Starting RR Services for each hand..." print "(Each process will open a new window)" full_command = []
def test_node_exist(self): nodes = rosnode.get_node_names() self.assertIn('/buzzer', nodes, "node dose not exist")
def test_node_exist(self): nodes = rosnode.get_node_names() self.assertIn('/wall_stop',nodes, "node does not exist")
def main (self): ##rospy.init_node("crane_x7_pick_and_place_controller") self.run = True self.robot = moveit_commander.RobotCommander() self.arm = moveit_commander.MoveGroupCommander("arm") self.gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) print("Group names:") print(self.robot.get_group_names()) print("Current state:") print(self.robot.get_current_state()) # アーム初期ポーズを表示 arm_initial_pose = self.arm.get_current_pose().pose print("Arm initial pose:") print(arm_initial_pose) """ 現在縦持ちで仮定しているためコメントアウト # 横持ち # タンバリンをたたくために位置移動 def preparation_horizontal(): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = tambourine_x_position_horizontal target_pose.position.y = tambourine_y_position_horizontal target_pose.position.z = tambourine_z_position_horizontal q = quaternion_from_euler(-3.14, 0, 3.14/2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行 # 角度を変化させタンバリンを叩く def hit_tambourine_horizontal(): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = tambourine_x_position_horizontal target_pose.position.y = tambourine_y_position_horizontal target_pose.position.z = tambourine_z_position_horizontal q = quaternion_from_euler(stick_angle_horizontal, 0, 3.14/2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行 """ # SRDFに定義されている"home"の姿勢にする self.arm.set_named_target("home") self.arm.go() target_pose = geometry_msgs.msg.Pose() target_pose.position.x = self.tambourine_x_position_vertical1 target_pose.position.y = self.tambourine_y_position_vertical1 target_pose.position.z = self.tambourine_z_position_vertical1 q = quaternion_from_euler(3.14 * 9 / 10, 3.14 / 2, -3.14) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行 self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() target_pose = geometry_msgs.msg.Pose() target_pose.position.x = self.tambourine_x_position_vertical1 target_pose.position.y = self.tambourine_y_position_vertical1 target_pose.position.z = self.tambourine_z_position_vertical1 q = quaternion_from_euler(3.14 * 9 / 10, 3.14 / 2, -3.14) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行 # SRDFに定義されている"home"の姿勢にする self.arm.set_named_target("home") self.arm.go() self.arm.set_max_velocity_scaling_factor(self.move_max_velocity_value) self.arm.set_named_target("home") self.arm.go() """ 現在縦持ちで仮定しているためコメントアウト #パターン-----横持ち self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.arm.set_max_velocity_scaling_factor(self.move_max_velocity_value) self.arm.set_named_target("home") self.arm.go() """ self.run = False
def update_model(self): """ query the ros master for information about the state of the system """ # Query master and compile a list of all published topics and their types allCurrentTopics = self._master.getPublishedTopics('/') allCurrentTopicNames = [x[0] for x in allCurrentTopics] # Get all the topics we currently know about rsgTopics = self._topology.topics # Remove any topics from Ros System Graph not currently known to master for topic in rsgTopics.values(): if topic.name not in allCurrentTopicNames: print "Removing Topic",topic.name, "not found in ",allCurrentTopicNames topic.release() # Add any topics not currently in the Ros System Graph for topicName, topicType in allCurrentTopics: if topicName not in rsgTopics: # and topicName not in QUIET_NAMES: topic = Topic(self._topology,topicName,topicType) # Compile a list of node names allCurrentNodes = rosnode.get_node_names() # Get all nodes we currently know about rsgNodes = self._topology.nodes # Remove any nodes from RosSystemGraph not currently known to master for node in rsgNodes.values(): if node.name not in allCurrentNodes: print "Removing Node",node.name, "not found in ",allCurrentNodes node.release() # Add any nodes not currently in the Ros System Graph for name in allCurrentNodes: if name not in rsgNodes: # and name not in QUIET_NAMES: node = Node(self._topology,name) try: node.location = self._master.lookupNode(name) except socket.error: raise Exception("Unable to communicate with master!") # Check for added or removed connections systemState = self._master.getSystemState() # Process publishers for topicName, publishersList in systemState[0]: if topicName in QUIET_NAMES: continue rsgPublishers = self._topology.topics[topicName].publishers # Remove publishers that don't exist anymore for publisher in rsgPublishers: if publisher.node.name not in publishersList: publisher.release() # Add publishers taht are not yet in the RosSystemGraph for nodeName in publishersList: if nodeName not in [pub.node.name for pub in rsgPublishers]: publisher = Publisher(self._topology,self._topology.nodes[nodeName],self._topology.topics[topicName]) # Process subscribers for topicName, subscribersList in systemState[1]: if topicName in QUIET_NAMES: continue try: rsgSubscribers = self._topology.topics[topicName].subscribers except: print topicName,"not found in" continue # Remove subscribers that don't exist anymore for subscriber in rsgSubscribers: if subscriber.node.name not in subscribersList: subscriber.release() # Add subscriber taht are not yet in the RosSystemGraph for nodeName in subscribersList: if nodeName not in [sub.node.name for sub in rsgSubscribers]: subscriber = Subscriber(self._topology,self._topology.nodes[nodeName],self._topology.topics[topicName]) self._update_view()
def test_node_exist(self): nodes = rosnode.get_node_names() self.assertIn('/lightsensors',nodes, "node does not exist")
def test_deadlock(self): # Prepare condition (for safe preemption) self.condition = threading.Condition() self.last_execution_time = None # Prepare Simple Action Server self.action_server = actionlib.SimpleActionServer( Constants.topic, TestAction, execute_cb=self.execute_callback, auto_start=False) self.action_server.register_preempt_callback(self.preempt_callback) self.action_server.start() # Sleep for the amount specified rospy.sleep(Constants.deadlock_timeout) # Start actual tests running_nodes = set(rosnode.get_node_names()) required_nodes = { "/deadlock_companion_1", "/deadlock_companion_2", "/deadlock_companion_3", "/deadlock_companion_4", "/deadlock_companion_5"} self.assertTrue(required_nodes.issubset(running_nodes), "Required companion nodes are not currently running") # Shutdown companions so that we can exit nicely termination_time = rospy.Time.now() rosnode.kill_nodes(required_nodes) rospy.sleep(Constants.shutdown_timeout) # Check last execution wasn't too long ago... self.assertIsNotNone(self.last_execution_time is None, "Execute Callback was never executed") time_since_last_execution = ( termination_time - self.last_execution_time).to_sec() self.assertTrue( time_since_last_execution < 2 * Constants.max_action_duration, "Too long since last goal was executed; likely due to a deadlock")