def read_topic(self, my_topic, device): device = device.lower() print 'my_topic = ' + my_topic.__str__() time_diff = rospy.Duration(1) time_previous_message = None # FIXME: The current timing code will probably have an increasing delay as messages are played. # We should fix this by basing the sleep duration on the difference between # the current wall clock and the desired time of the message print( '\nmsg count = ' + self.bag.get_message_count().__str__()) bag_messages = self.bag.read_messages( topics=[my_topic]) for topic, msg, time_current_message in bag_messages: if device == "hardware": if self.read_count == 0: self.set_state(topic, 'DVRK_POSITION_GOAL_JOINT') elif self.read_count == 3: self.set_state(topic, 'DVRK_POSITION_JOINT') self.read_count = self.read_count + 1 # print(self.read_count) # the timestamp of each recorded message is saved as time_current_message if time_previous_message != None: time_diff = time_current_message-time_previous_message temp_t = time.time() sleep(time_diff) # s = raw_input('\nPress any key to continue...(except q)') # if s == 'q': # return print('time.time says {}\n'.format(time.time() - temp_t)) print( 'time_diff = {} '.format(time_diff.to_sec() ) ) # print('time_previous_message = {}, rostime = {}'.format(time_previous_message, rospy.rostime.get_rostime())) time_previous_message= time_current_message if 'PSM1' in topic or 'PSM2' in topic: msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw'] elif 'ECM' in topic: msg.name = ['outer_yaw', 'outer_pitch', 'insertion', 'outer_roll'] # publish if self.read_count < 3 and device == "hardware": print('HOMING..\n') for arm_name in ARM_NAMES: if arm_name in topic: print(arm_name + topic) rospy.Publisher('/dvrk/{}/set_position_goal_joint'.format(arm_name), JointState, queue_size=10).publish(msg) sleep(2) else: if device == "simulation": self.pub_sim[topic].publish(msg) if device == "hardware": msg.position = [ round(i,4) for i in msg.position] self.pub_hw[topic].publish(msg)
def __init__(self, arm_names): if type(arm_names) == type(""): arm_names = [arm_names] # initialize a bag self.bag = rosbag.Bag('test.bag', 'w') self.arm_names = arm_names # The topics we want to record # self.topics = '/dvrk/{}/position_joint_current'.format(ARM_NAME) self.topics = { arm_name: '/dvrk/{}/position_joint_current'.format(arm_name) for arm_name in self.arm_names } self.out_topics_hw = { arm_name: '/dvrk/{}/set_position_joint'.format(arm_name) for arm_name in self.arm_names } self.out_topics_sim = { arm_name: '/dvrk_{}/joint_states_robot'.format(arm_name.lower()) for arm_name in self.arm_names } # self.out_topics = self.out_topics_sim self.out_topics = self.topics print self.topics # We have to initialize a ros node if we want to subsribe or publish messages rospy.init_node('rosbag_test_node') for arm_name in self.arm_names: eval("rospy.Subscriber('{}', JointState, self.cb_{})".format( self.topics[arm_name], arm_name)) sleep(.1) rospy.spin()
def writeTodbm(self): self.db = dbm.open(self.db_file, 'w') self.timer = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d %H:%M:%S') self.db['timer'] = self.timer #写能量数据 self.db['eg_total_sum'] = str(self.eg_total_sum) self.db['eg_min_sum'] = str(self.eg_min_sum) self.db['eg_max_sum'] = str(self.eg_max_sum) #写过零数据 self.db['zc_total_sum'] = str(self.zc_total_sum) self.db['zc_min_sum'] = str(self.zc_min_sum) self.db['zc_max_sum'] = str(self.zc_max_sum) #如果误差范围很小,则可以休眠一段时间,并将消息写入json文件 eg_err = float( self.db['eg_min_avg']) - (self.eg_min_sum / self.eg_numbers * 10) zc_err = float( self.db['zc_min_avg']) - (self.zc_min_sum / self.zc_numbers * 10) self.db['eg_total_avg'] = str(self.eg_total_sum / self.eg_numbers) self.db['eg_min_avg'] = str(self.eg_min_sum / self.eg_numbers * 10) self.db['eg_max_avg'] = str(self.eg_max_sum / self.eg_numbers * 10) self.db['eg_numbers'] = str(self.eg_numbers) self.db['zc_total_avg'] = str(self.zc_total_sum / self.zc_numbers) self.db['zc_min_avg'] = str(self.zc_min_sum / self.zc_numbers * 10) self.db['zc_max_avg'] = str(self.zc_max_sum / self.zc_numbers * 10) self.db['zc_numbers'] = str(self.zc_numbers) #若同时收敛,则写如json 文件并且发布消息 if ((-0.01 < eg_err and eg_err < 0.01) and (-1 < zc_err and zc_err < 1)): with open(os.path.join(self.wkdir, "eg_zc_average.json"), 'w') as fd: self.ezdir["eg_total_avg"] = float(self.db['eg_total_avg']) self.ezdir['eg_min_avg'] = float(self.db['eg_min_avg']) self.ezdir['eg_max_avg'] = float(self.db['eg_max_avg']) self.ezdir["zc_total_avg"] = float(self.db['zc_total_avg']) self.ezdir['zc_min_avg'] = float(self.db['zc_min_avg']) self.ezdir['zc_max_avg'] = float(self.db['zc_max_avg']) fd.write(json.dumps(self.ezdir)) fd.close() self.state = True if self.state: self.pub.publish("update") self.state = False sleep(1) self.db.close()
def LaunchNode(packageName, launchFile, nodesList, timeOut=10): print "launching: " + launchFile my_env = os.environ.copy() my_env["TURTLEBOT_3D_SENSOR"] = "commonsense" my_env["TURTLEBOT_STACKS"] = "hexagons" proc = subprocess.Popen(['roslaunch', packageName, launchFile], env=my_env) allNodes = 0 while allNodes != len(nodesList) and timeOut > 0: allNodes = 0 sleep(1) for node in nodesList: if rosnode.rosnode_ping(node, 1, False) == True: allNodes = allNodes + 1 else: break timeOut = timeOut - 1 print "Process pid: " + str(proc.pid) return proc.pid
def __init__(self): # initialize a bag self.bag = rosbag.Bag('test.bag', 'w') # The topics we want to record # self.topics = '/dvrk/{}/position_joint_current'.format(ARM_NAME) self.topics = { arm_name:'/dvrk/{}/position_joint_current'.format(arm_name) for arm_name in ARM_NAMES} self.out_topics = {arm_name : '/dvrk/{}/set_position_joint'.format(arm_name) for arm_name in ARM_NAMES} self.out_topics = self.topics print self.topics # We have to initialize a ros node if we want to subsribe or publish messages rospy.init_node('rosbag_test_node') for arm_name in ARM_NAMES: eval("rospy.Subscriber('{}', JointState, self.cb_{})".format(self.topics[arm_name], arm_name)) sleep(.1) rospy.spin()
def main(): a = rospy.init_node('set_base_frames') global psm1_kin, psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot, mtmr_robot, mtmr_kin if psm1_robot is None: psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description') psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[1].name) if psm2_robot is None: psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description') psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[0].name, psm2_robot.links[1].name) if ecm_robot is None: ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description') ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[-1].name) ecm_base = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[3].name) if mtmr_robot is None: mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description') mtmr_base = KDLKinematics(mtmr_robot, mtmr_robot.links[0].name, mtmr_robot.links[1].name) # pdb.set_trace() psm1_pub = rospy.Publisher('/dvrk/PSM1/set_base_frame', Pose, latch=True, queue_size=1) psm2_pub = rospy.Publisher('/dvrk/PSM2/set_base_frame', Pose, latch=True, queue_size=1) mtmr_pub = rospy.Publisher('/dvrk/MTMR/set_base_frame', Pose, latch=True, queue_size=1) ecm_tip = ecm_kin.forward([1,1,1,1]) # ECM Tool Tip ecm_tip = np.linalg.inv(ecm_tip) psm1_base_frame = psm1_kin.forward([]) psm1_message = pose_converter.PoseConv.to_pose_msg(psm1_base_frame) psm2_base_frame = psm2_kin.forward([]) psm2_message = pose_converter.PoseConv.to_pose_msg(psm2_base_frame) psm1_pub.publish(psm1_message) psm2_pub.publish(psm2_message) # mtmr_pub.publish(message) print (psm1_message) print (psm2_message) sleep (1)
def __init__(self, arm_names): if type(arm_names) == type(""): arm_names = [arm_names] # initialize a bag self.bag = rosbag.Bag('test.bag', 'w') self.arm_names = arm_names # The topics we want to record # self.topics = '/dvrk/{}/position_joint_current'.format(ARM_NAME) self.topics = { arm_name:'/dvrk/{}/position_joint_current'.format(arm_name) for arm_name in self.arm_names} self.out_topics_hw = {arm_name : '/dvrk/{}/set_position_joint'.format(arm_name) for arm_name in self.arm_names} self.out_topics_sim = {arm_name : '/dvrk_{}/joint_states_robot'.format(arm_name.lower()) for arm_name in self.arm_names} # self.out_topics = self.out_topics_sim self.out_topics = self.topics print self.topics # We have to initialize a ros node if we want to subsribe or publish messages rospy.init_node('rosbag_test_node') for arm_name in self.arm_names: eval("rospy.Subscriber('{}', JointState, self.cb_{})".format(self.topics[arm_name], arm_name)) sleep(.1) rospy.spin()
def move(timer_event): """ handles the movement """ global current_state global is_currently_moving if rospy.get_param("KEY"): # wait if there is a keyboard command sleep(2) if is_currently_moving and (not movement_is_ok()): return if current_state == FWD: move_robot(DISTANCE_BEFORE_TURN) current_state = TRN else: # calculate a random angle to turn angle = random.randrange(-MAX_TURNING_ANGLE, MAX_TURNING_ANGLE) clockwise = angle < 0 turn_robot(angle,clockwise) current_state = FWD
def main(): rospy.init_node('set_base_frames') sleep (1) global psm1_kin, psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot if psm1_robot is None: psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description') psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[1].name) if psm2_robot is None: psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description') psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[0].name, psm2_robot.links[1].name) if ecm_robot is None: ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description') ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[-1].name) ecm_base = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[3].name) # pdb.set_trace() mtml_pub = rospy.Publisher('/dvrk/MTML/set_base_frame', Pose, queue_size=1) mtmr_pub = rospy.Publisher('/dvrk/MTMR/set_base_frame', Pose, queue_size=1) ecm_pub = rospy.Publisher('/dvrk/ECM/set_base_frame', Pose, queue_size=1) psm1_pub = rospy.Publisher('/dvrk/PSM1/set_base_frame', Pose, queue_size=1) psm2_pub = rospy.Publisher('/dvrk/PSM2/set_base_frame', Pose, queue_size=1) mtmr_psm1 = rospy.Publisher('/dvrk/MTMR_PSM1/set_registration_rotation', Quaternion, queue_size=1) mtml_psm2 = rospy.Publisher('/dvrk/MTML_PSM2/set_registration_rotation', Quaternion, queue_size=1) p1_base = psm1_kin.forward([]) # PSM1 Base Frame p2_base = psm2_kin.forward([]) # PSM2 Base Frame e_base = ecm_base.forward([]) # ECM Base Frame e = ecm_kin.forward([0,0,0,0]) # ECM Tool Tip camera_view_transform = pose_converter.PoseConv.to_homo_mat( [(0.0,0.0,0.0), (1.57079632679, 0.0, 0.0)]) r = lambda axis, rad: rotate(axis, rad) mtmr_m = e;# mtmr_m = mtmr_m**-1 mtml_m = e print 'qmat' qmsg = Quaternion() temp = quat_from_homomat(p1_base) print quat_from_homomat(p1_base) message = pose_from_homomat(p1_base); psm1_pub.publish(message) while not rospy.is_shutdown(): #print p1_base #print message psm1_pub.publish(message) print ('sure\n') # psm2_pub.publish(pose_from_homomat(p2_base)) # psm1_pub.publish(pose_from_homomat(e_base)) # mtml_pub.publish( pose_from_homomat(mtml_m)) # mtmr_pub.publish( pose_from_homomat(mtmr_m)) print ('\n\Hello: nmtml rotation: \n') print( mtml_m.__repr__( )) print(pose_from_homomat(mtml_m)) print ('\n\nmtmr rotation: \n') print( mtmr_m.__repr__())
def run_as_process(aut: AutomatonBase, i_queue: Queue, o_queue: Queue, stop_ev: Event, **kwargs: Any) -> None: """ NOTE: Must be executed in the main thread of a process """ start_time = float("NaN") try: rospy.init_node(repr(aut), anonymous=True, disable_signals=False) rospy.logdebug("%s: waiting for the first clock message" % repr(aut)) rospy.wait_for_message("/clock", Clock) # Wait for first update of clock # Initialize Publishers and Subscribers # FIXME TIOA should not depend on specific implementations from .agent import Agent if isinstance(aut, Agent): aut.motion.register_ros_pub_sub() pose_topic_name = "/vrpn_client_node/%s/pose" % str(aut.uid) def update_pose(data: PoseStamped): aut.motion.position = data.pose.position aut.motion.orientation = data.pose.orientation # NOTE This creates a thread in this process rospy.Subscriber(pose_topic_name, PoseStamped, update_pose, queue_size=10) rospy.logdebug("%s: waiting for the first position message" % repr(aut)) rospy.wait_for_message(pose_topic_name, PoseStamped) busy_waiting_start = rospy.Time.now() start_time = busy_waiting_start.to_sec() rospy.logdebug("Start %s at %.2f" % (aut, busy_waiting_start.to_sec())) while not stop_ev.is_set() and not aut.reached_sink_state(): sleep(0.0) # Yield to other threads # TODO avoid each iteration of while loop running indefinitely long # Select an enabled action act = _select_act(aut, i_queue) # No enabled action, stay at current discrete state # NOTE clock and continuous variables are still updated if act is not None: busy_waiting_start = rospy.Time.now() else: timeout = 300 if busy_waiting_start + rospy.Duration(secs=timeout) < rospy.Time.now(): print("Busy waiting for over %d seconds without new actions." % timeout, end=' ') break continue # Send output action to the environment if aut.is_output(act): o_queue.put_nowait(act) # Run transition of automaton aut.transition(act) except KeyboardInterrupt: print("KeyboardInterrupt.", end=' ') # except RuntimeError as e: # print(repr(e), end=' ') finally: o_queue.close() i_queue.close() end_time = rospy.Time.now().to_sec() rospy.logdebug("Ending %s at %.2f..." % (aut, end_time)) print('-', {"name": repr(aut), "t_start": start_time, "t_end": end_time, **aut.queries}) rospy.signal_shutdown("Shutting down ROS node for %s" % aut)
# * terms of the Eclipse Public License 2.0 which is available at # * http://www.eclipse.org/legal/epl-2.0. # * # * SPDX-License-Identifier: EPL-2.0 # * # * Contributors: # * Daniel Heß # ******************************************************************************** import math import rospy from adore_if_ros_msg.msg import NavigationGoal from rospy.timer import sleep if __name__ == '__main__': rospy.init_node('adore_set_goal', anonymous=True) output = 'ENV/NavigationGoal' marker_name = rospy.get_param('~marker') x0 = rospy.get_param(marker_name + '_x0', 0.0) y0 = rospy.get_param(marker_name + '_y0', 0.0) z0 = rospy.get_param(marker_name + '_z0', 0.0) pub = rospy.Publisher(output, NavigationGoal, queue_size=1) g = NavigationGoal() g.target.x = x0 g.target.y = y0 g.target.z = z0 while pub.get_num_connections() < 2: sleep(0.1) for i in range(10): pub.publish(g) sleep(1.0) rospy.spin()
def read_topic(self, my_topic, device): device = device.lower() print 'my_topic = ' + my_topic.__str__() time_diff = rospy.Duration(1) time_previous_message = None # FIXME: The current timing code will probably have an increasing delay as messages are played. # We should fix this by basing the sleep duration on the difference between # the current wall clock and the desired time of the message print('\nmsg count = ' + self.bag.get_message_count().__str__()) bag_messages = self.bag.read_messages(topics=[my_topic]) for topic, msg, time_current_message in bag_messages: if device == "hardware": if self.read_count == 0: self.set_state(topic, 'DVRK_POSITION_GOAL_JOINT') elif self.read_count == 3: self.set_state(topic, 'DVRK_POSITION_JOINT') self.read_count = self.read_count + 1 # print(self.read_count) # the timestamp of each recorded message is saved as time_current_message if time_previous_message != None: time_diff = time_current_message - time_previous_message temp_t = time.time() sleep(time_diff) # s = raw_input('\nPress any key to continue...(except q)') # if s == 'q': # return print('time.time says {}\n'.format(time.time() - temp_t)) print('time_diff = {} '.format(time_diff.to_sec())) # print('time_previous_message = {}, rostime = {}'.format(time_previous_message, rospy.rostime.get_rostime())) time_previous_message = time_current_message if 'PSM1' in topic or 'PSM2' in topic: msg.name = [ 'outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw' ] elif 'ECM' in topic: msg.name = [ 'outer_yaw', 'outer_pitch', 'insertion', 'outer_roll' ] elif 'MTM' in topic: msg.name = [ 'outer_yaw', 'shoulder_pitch', 'shoulder_pitch_parallel', 'elbow_pitch', 'wrist_platform', 'wrist_pitch', 'wrist_yaw', 'wrist_roll' ] # publish if self.read_count < 3 and device == "hardware": print('HOMING..\n') if self.arm_name in topic: print(self.arm_name + topic) rospy.Publisher('/dvrk/{}/set_position_goal_joint'.format( self.arm_name), JointState, queue_size=10).publish(msg) sleep(2) else: if device == "simulation": self.pub_sim[topic].publish(msg) if device == "hardware": msg.position = [round(i, 4) for i in msg.position] self.pub_hw[topic].publish(msg)
# * Daniel Heß # ******************************************************************************** import math import rospy from geometry_msgs.msg import Pose from rospy.timer import sleep if __name__ == '__main__': rospy.init_node('adore_set_pose', anonymous=True) output = 'SIM/ResetVehiclePose' marker_name = rospy.get_param('~marker') x0 = rospy.get_param(marker_name + '_x0', 0.0) y0 = rospy.get_param(marker_name + '_y0', 0.0) z0 = rospy.get_param(marker_name + '_z0', 0.0) x1 = rospy.get_param(marker_name + '_x1', 1.0) y1 = rospy.get_param(marker_name + '_y1', 0.0) z1 = rospy.get_param(marker_name + '_z1', 0.0) pub = rospy.Publisher(output, Pose, queue_size=1) p0 = Pose() p0.position.x = x0 p0.position.y = y0 p0.position.z = z0 yaw = math.atan2(y1 - y0, x1 - x0) p0.orientation.x = 0.0 p0.orientation.y = 0.0 p0.orientation.z = math.sin(yaw * 0.5) p0.orientation.w = math.cos(yaw * 0.5) while pub.get_num_connections() == 0: sleep(0.1) pub.publish(p0) rospy.spin()
#random = 0 #time = rospy.get_rostime() newtime = time print "beginning loop" #while(key != '-' and key != '\x03'): while(time < 80 and key != '\x03'): # print msg if (key == '\x03'): break else: print "turn "+i twist = Twist() twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = turn pub.publish(twist) timer.sleep(.8) time = time +1 #newtime = rospy.get_rostime() print "loop ended" twist = Twist() twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0 pub.publish(twist) print "Finished first round" if (key == '\x03'): break else: key = ' '