def __init__(self): self.tfBcaster = tf2_ros.TransformBroadcaster() # FIXME, use that for ENU to NED self.static_tfBcaster = tf2_ros.StaticTransformBroadcaster() self.tfBuffer = tf2_ros.Buffer() self.tfLstener = tf2_ros.TransformListener(self.tfBuffer)
Roslaunch usage: <node pkg="dnb_tf" type="tf_chain_publisher.py" name="TODO"> <param name="root_frame" value="base_link"/> <param name="tip_frame" value="ee_link"/> <remap from="tf_chain" to="tool_state"/> </node> """ if __name__ == '__main__': rospy.init_node('tf_chain_publisher') rate = rospy.Rate(100) tr = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tr) topic = rospy.resolve_name('tf_chain') topic2 = rospy.resolve_name('tool_frame') publish = rospy.Publisher(topic, EulerFrame, queue_size=1).publish publish2 = rospy.Publisher(topic2, EulerFrame, queue_size=1).publish # Note that frame tip in root = transform root to tip target_frame = rospy.get_param('~root_frame', 'base_link') source_frame = rospy.get_param('~tip_frame', 'ee_link') # wait a second for frames to accumulate. buf.lookup_transform seems to fail immediately if it hasn't yet # gotten any transfroms for the 'odom' frame, instead of waiting for the timeout rospy.sleep(1) while not rospy.is_shutdown():
def __init__(self): self.tilt_eye_pub = rospy.Publisher("/eye_tilt", Float64, queue_size=1) self.pan_eye_left_pub = rospy.Publisher("/left_eye_pan", Float64, queue_size=1) self.pan_eye_right_pub = rospy.Publisher("/right_eye_pan", Float64, queue_size=1) self.tilt_head_pub = rospy.Publisher("/neck_pitch", Float64, queue_size=1) self.pan_head_pub = rospy.Publisher("neck_yaw", Float64, queue_size=1) self.roi_pub = rospy.Publisher("/roi", Image, queue_size=1) self.label_pub = rospy.Publisher("/label", String, queue_size=1) self.probe_pub = rospy.Publisher("/probe_results", String, queue_size=1) self.point_pub = rospy.Publisher("/saccade_point", PointStamped, queue_size=1) self.tilt_pub = rospy.Publisher("/tilt", Float64, queue_size=1) self.pan_pub = rospy.Publisher("/pan", Float64, queue_size=1) self.shift_pub = rospy.Publisher("/shift", std_msgs.msg.Empty, queue_size=1) self.status_pub = rospy.Publisher("/status", String, queue_size=1) self.recognizer = rospy.ServiceProxy('recognize', Roi) self.memory = rospy.ServiceProxy('new_object', NewObject) self.probe_label = rospy.ServiceProxy('probe_label', ProbeLabel) self.probe_coordinate = rospy.ServiceProxy('probe_coordinate', ProbeCoordinate) self.saccade_ser = rospy.Service('saccade', Target, self.saccade) self.look_ser = rospy.Service('look', Look, self.look) self.probe_ser = rospy.Service('probe', SetBool, self.probe) self.transform_ser = rospy.Service('transform', Transform, self.transform) self.camera_image = None self.camera_info_left = None self.camera_info_right = None self.disparity_image = None self.camera_model = StereoCameraModel() self.link_states = None self.tilt_eye = 0. self.pan_eye = 0. self.tilt_head = 0. self.pan_head = 0. self.tilt_eye_upper_limit = 0.4869469 self.tilt_eye_lower_limit = -0.8220501 self.pan_eye_limit = 0.77754418 self.tilt_head_limit = math.pi/4 self.pan_head_limit = math.pi/2 self.saliency_width = float(rospy.get_param('~saliency_width', '256')) self.saliency_height = float(rospy.get_param('~saliency_height', '192')) self.move_eyes = rospy.get_param("~move_eyes", True) self.move_head = rospy.get_param("~move_head", True) self.shift = rospy.get_param("~shift", True) self.min_disparity = rospy.get_param("/camera/stereo_image_proc/min_disparity", "-16") self.recognize = rospy.get_param("~recognize", True) self.probe = rospy.get_param("~probe", False) self.cv_bridge = CvBridge() self.tfBuffer = tf2_ros.Buffer(rospy.Duration(30)) self.listener = tf2_ros.TransformListener(self.tfBuffer) self.transform = None self.static_frame = "hollie_base_x_link" camera_sub = rospy.Subscriber("/camera_left/image_raw", Image, self.image_callback, queue_size=1, buff_size=2**24) camera_info_left_sub = rospy.Subscriber("/camera_left/camera_info", CameraInfo, self.camera_info_left_callback, queue_size=1, buff_size=2**24) camera_info_right_sub = rospy.Subscriber("/camera_right/image _info", CameraInfo, self.camera_info_right_callback, queue_size=1, buff_size=2**24) disparity_sub = rospy.Subscriber("/camera/disparity", DisparityImage, self.disparity_callback, queue_size=1, buff_size=2**24) joint_state_sub = rospy.Subscriber("/joint_states", JointState, self.joint_state_callback, queue_size=1, buff_size=2**24) link_state_sub = rospy.Subscriber("/gazebo/link_states", LinkStates, self.link_state_callback, queue_size=1, buff_size=2**24)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # We need a tf2 listener to convert poses into arm reference base try: self._tf2_buff = tf2_ros.Buffer() self._tf2_list = tf2_ros.TransformListener(self._tf2_buff) except rospy.ROSException as err: rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err)) raise err self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01] self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01] self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral", (self.gripper_opened[0] + self.gripper_closed[0])/2.0) ] self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0) # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 1 # Set a limit on the number of place attempts max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) rospy.loginfo("Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") rospy.sleep(1) arm.set_named_target('right_up') if arm.go() != True: rospy.logwarn(" Go failed") gripper.set_joint_value_target(self.gripper_opened) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(1) # Initialize the grasping goal goal = FindGraspableObjectsGoal() goal.plan_grasps = False find_objects.send_goal(goal) find_objects.wait_for_result(rospy.Duration(5.0)) find_result = find_objects.get_result() rospy.loginfo("Found %d objects" %len(find_result.objects)) for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() self.scene._colors = dict() # Use the nearest object on the table as the target target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_id = 'target' target_size = None the_object = None the_object_dist = 0.30 the_object_dist_min = 0.10 count = -1 for obj in find_result.objects: count +=1 dx = obj.object.primitive_poses[0].position.x dy = obj.object.primitive_poses[0].position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < the_object_dist and d > the_object_dist_min: #if dx > the_object_dist_xmin and dx < the_object_dist_xmax: # if dy > the_object_dist_ymin and dy < the_object_dist_ymax: rospy.loginfo("object is in the working zone") the_object_dist = d the_object = count target_size = [0.02, 0.02, 0.05] target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0 target_pose.pose.position.y = obj.object.primitive_poses[0].position.y target_pose.pose.position.z = 0.04 target_pose.pose.orientation.x = 0.0 target_pose.pose.orientation.y = 0.0 target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 1.0 # wait for the scene to sync self.scene.waitForSync() self.scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) self.scene.sendColors() grasp_pose = target_pose grasp_pose.pose.position.y -= target_size[1] / 2.0 grasp_pose.pose.position.x += target_size[0] grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten]) # Track success/failure and number of attempts for pick operation for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) else: rospy.loginfo("object is not in the working zone") rospy.sleep(1) arm.set_named_target('right_up') arm.go() # Open the gripper to the neutral position gripper.set_joint_value_target(self.gripper_neutral) gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def init(self): # save terminal settings self.settings = termios.tcgetattr(sys.stdin) self.tfBuffer = tf2_ros.Buffer() self.tfListener = tf2_ros.TransformListener(self.tfBuffer) # noqa self.footstep_count = 0 # create publisher for arm trajectories robot_name = "valkyrie" #rospy.get_param('/ihmc_ros/robot_name') self.arm_publisher = rospy.Publisher( '/ihmc_ros/{0}/control/arm_trajectory'.format(robot_name), ArmTrajectoryRosMessage, queue_size=1) self.left_hand_publisher = rospy.Publisher( '/left_hand_position_controller/command', Float64MultiArray, queue_size=1) self.right_hand_publisher = rospy.Publisher( '/right_hand_position_controller/command', Float64MultiArray, queue_size=1) self.neck_publisher = rospy.Publisher( '/ihmc_ros/{0}/control/neck_trajectory'.format(robot_name), NeckTrajectoryRosMessage, queue_size=1) self.head_publisher = rospy.Publisher( '/ihmc_ros/{0}/control/head_trajectory'.format(robot_name), HeadTrajectoryRosMessage, queue_size=1) self.footstep_publisher = rospy.Publisher( '/ihmc_ros/{0}/control/footstep_list'.format(robot_name), FootstepDataListRosMessage, queue_size=1) self.footstep_status_subscriber = rospy.Subscriber( '/ihmc_ros/{0}/output/footstep_status'.format(robot_name), FootstepStatusRosMessage, self.receivedFootStepStatus_cb) self.abort_walking_publisher = rospy.Publisher( '/ihmc_ros/{0}/control/abort_walking'.format(robot_name), AbortWalkingRosMessage, queue_size=1) # Declare Footstep command Visualizer self.footstep_visualize_publisher = rospy.Publisher( '/visualize_footstep_teleop', MarkerArray, queue_size=1) # right_foot_frame_parameter_name = "/ihmc_ros/{0}/right_foot_frame_name".format(robot_name) # left_foot_frame_parameter_name = "/ihmc_ros/{0}/left_foot_frame_name".format(robot_name) # if rospy.has_param(right_foot_frame_parameter_name) and \ # rospy.has_param(left_foot_frame_parameter_name): # self.RIGHT_FOOT_FRAME_NAME = rospy.get_param(right_foot_frame_parameter_name) # self.LEFT_FOOT_FRAME_NAME = rospy.get_param(left_foot_frame_parameter_name) # make sure the simulation is running otherwise wait self.rate = rospy.Rate(2) # 2hz publishers = [self.neck_publisher] # self.arm_publisher, self.left_hand_publisher, self.right_hand_publisher, # self.neck_publisher, self.head_publisher, self.footstep_publisher] if any([p.get_num_connections() == 0 for p in publishers]): while any([p.get_num_connections() == 0 for p in publishers]): rospy.loginfo('waiting for subscribers: ' + ', '.join( sorted([ p.name for p in publishers if p.get_num_connections() == 0 ]))) self.rate.sleep()
marker_odom = tf_buf.transform(marker, 'map', rospy.Duration(0.5)) trans = TransformStamped() trans.header.stamp = rospy.Time.now() trans.child_frame_id = "aruco/detected" + str(marker_detect.id) trans.header.frame_id = 'map' trans.transform.translation = marker_odom.pose.position trans.transform.rotation = marker_odom.pose.orientation br.sendTransform(trans) rospy.init_node("aruco_tranz") tf_buf = tf2_ros.Buffer() tf_lstn = tf2_ros.TransformListener(tf_buf) br = tf2_ros.StaticTransformBroadcaster() sub_det = rospy.Subscriber('/aruco/markers', MarkerArray, goal_callback) def main(): rate = rospy.Rate(20) while not rospy.is_shutdown(): if goal: transform_marker(goal) rate.sleep() if __name__ == '__main__': main()
if __name__ == "__main__": rospy.init_node("cor") topico_imagem = "/camera/image/compressed" recebedor = rospy.Subscriber(topico_imagem, CompressedImage, roda_todo_frame, queue_size=4, buff_size=2**24) print("Usando ", topico_imagem) velocidade_saida = rospy.Publisher("/cmd_vel", Twist, queue_size=1) tfl = tf2_ros.TransformListener( tf_buffer) #conversao do sistema de coordenadas tolerancia = 25 # Exemplo de categoria de resultados # [('chair', 86.965459585189819, (90, 141), (177, 265))] try: # Inicializando - por default gira no sentido anti-horário # vel = Twist(Vector3(0,0,0), Vector3(0,0,math.pi/10.0)) while not rospy.is_shutdown(): for r in resultados: print(r) #velocidade_saida.publish(vel) rospy.sleep(0.1)
def __init__(self, context): super(Camera_is1500_Widget, self).__init__(context) # Give QObjects reasonable names self.setObjectName('Camera setting') # Process standalone plugin command-line arguments from argparse import ArgumentParser parser = ArgumentParser() # Add argument(s) to the parser. parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode") args, unknowns = parser.parse_known_args(context.argv()) if not args.quiet: print 'arguments: ', args print 'unknowns: ', unknowns # Create QWidget self._widget = QWidget() # Get path to UI file which should be in the "resources" folder of this package ui_file = os.path.join(rospkg.RosPack().get_path('rqt_camera_is1500'), 'resources', 'Camera_is1500_Widget.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('Camera_is1500_WidgetUI') # Show _widget.windowTitle on left-top of each plugin (when # it's set in _widget). This is useful when you open multiple # plugins at once. Also if you open multiple instances of your # plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) # Add widget to the user interface context.add_widget(self._widget) """ Start nodes """ self.target_x = 0.0 self.target_y = 0.0 self.distance_x = 0.0 """ MAP path for is1500 package """ self.map_path_name = '' self.map_file_name = '' self.destination_map_file_name = '' self.destination_map_config_file_name = '' self.cameramap = [] self.current_wp = None """ RVIZ-APPs """ self.map_forRviz_file_name = self._widget.map_to_rviz_path_edit.text() self.utm_x_value = float(self._widget.utm_x_edit.text()) self.utm_y_value = float(self._widget.utm_y_edit.text()) self.utm_phi_value = float(self._widget.utm_phi_edit.text()) self.joao_origin_utm_x_value = float( self._widget.joao_origin_utm_x_edit.text()) self.joao_origin_utm_y_value = float( self._widget.joao_origin_utm_y_edit.text()) self.fiducials_map = [ ] # contain the position of the fiducial in camera frame self.indoor = self._widget.indoor_checkBox_edit.isChecked() self.init_transf = False # True if map already drew self.origin_cam_x = 0.0 self.origin_cam_y = 0.0 self.last_robot_x_edit = 0.0 #float(self._widget.get_pos_robot_x_edit.text()) self.last_robot_y_edit = 0.0 #float(self._widget.get_pos_robot_y_edit.text()) self.last_robot_yaw_edit = 0.0 #float(self._widget.get_pos_robot_yaw_edit.text()) self.angle_btwXcam_East = 0.0 """ Connect stuff here """ # self.model = QStandardItemModel(self._widget.arc_list) # self._widget.name_edit.setValidator(QDoubleValidator()) # self._widget.path_edit.setValidator(QDoubleValidator()) # self._widget.in_case_edit.setValidator(QDoubleValidator()) # self._widget.y_edit.setValidator(QDoubleValidator()) # self._widget.map_name_edit.setValidator() """ Start nodes """ self._widget.launch_camera_start_button.released.connect( self.start_camera_is1500_launchFile) self._widget.color_launch_camera_label.setStyleSheet( "background-color:#ff0000;") self._widget.launch_supervisor_start_button.released.connect( self.start_supervisor_launchFile) self._widget.launch_supervisor_color_label.setStyleSheet( "background-color:#ff0000;") self._widget.rviz_start_button.released.connect(self.start_rviz) self._widget.color_rviz_label.setStyleSheet( "background-color:#ff0000;") self._widget.target_start_button.released.connect( self.target_waypontPy_run) self._widget.color_target_label.setStyleSheet( "background-color:#ff0000;") self._widget.distance_start_button.released.connect( self.distance_throughDeadZone_run) self._widget.color_distance_label.setStyleSheet( "background-color:#ff0000;") self._widget.test_button.released.connect(self.test_button_function) """ Set map """ self._widget.send_map_param_button.released.connect( self.set_map_rosparam) self._widget.map_param_edit.setValidator(QDoubleValidator()) """ Add/modify map """ self._widget.file_name_button.released.connect(self.get_file) self._widget.desination_file_name_button.released.connect( self.get_folder) self._widget.desination_save_file_button.released.connect( self.save_file) self._widget.config_map_file_name_button.released.connect( self.get_file_yaml) self._widget.config_map_save_file_button.released.connect( self.config_save_file) self.model = QStandardItemModel(self._widget.map_from_mapYaml_list) self.model.itemChanged.connect(self.on_change_mapList) # Screen explaination msg # self._widget.name_edit.setToolTip("Set the name of the folder which will contain the map ") # self._widget.path_edit.setToolTip("Path of th map") # self._widget.file_name_label.setToolTipe("Map file which be added to the map folder of the robot") # Doesn't work # self._widget.name_edit.description = "This is label name_edit" # self._widget.path_edit.description = "This is label path_edit" # self._widget.file_name_button.description = "This is the OK button" # # for widget in (self._widget.name_edit, self._widget.path_edit, # self._widget.file_name_button): # widget.bind("<Enter>", self.on_enter) # widget.bind("<Leave>", self.on_leave) self._widget.map_name_edit.textChanged.connect(self.map_name_change) """ Rviz part """ self._widget.utm_x_edit.setValidator(QDoubleValidator()) self._widget.utm_y_edit.setValidator(QDoubleValidator()) self._widget.utm_phi_edit.setValidator(QDoubleValidator()) self._widget.utm_x_edit.textChanged.connect(self.utm_x_change) self._widget.utm_y_edit.textChanged.connect(self.utm_y_change) self._widget.utm_phi_edit.textChanged.connect(self.utm_phi_change) self._widget.joao_origin_utm_x_edit.setValidator(QDoubleValidator()) self._widget.joao_origin_utm_y_edit.setValidator(QDoubleValidator()) self._widget.joao_origin_utm_x_edit.textChanged.connect( self.joao_origin_utm_x_change) self._widget.joao_origin_utm_y_edit.textChanged.connect( self.joao_origin_utm_y_change) self._widget.indoor_checkBox_edit.stateChanged.connect( self.indoor_isCheck) self._widget.reset_init_map_file_button.released.connect( self.reset_init_change) self._widget.get_position_button.released.connect(self.get_pos_change) self._widget.get_pos_robot_x_edit.setValidator(QDoubleValidator()) self._widget.get_pos_robot_y_edit.setValidator(QDoubleValidator()) self._widget.get_pos_robot_yaw_edit.setValidator(QDoubleValidator()) # Buttons self._widget.map_to_rviz_send_file_button.released.connect( self.visualize_fiducials) self._widget.map_to_rviz_name_button.released.connect( self.get_file_map_to_rviz) """ ROS """ self.marker_pub = rospy.Publisher( '/fiducials_position', visualization_msgs.msg.MarkerArray, queue_size=10) # publish fiducials for a chosen map self.currentMap_marker_pub = rospy.Publisher( '/fiducials_position_current', visualization_msgs.msg.MarkerArray, queue_size=10) #publish currend used fiducials self.camera_pos_sub = rospy.Subscriber("/base_link_odom_camera_is1500", Odometry, self.publish_transform_pos) # self.camera_pos_sub = rospy.Subscriber("/position_camera_is1500", Odometry, self.publish_transform_pos) self.transform_cameraPos_pub = rospy.Publisher( '/transform_cameraPos_pub', Odometry, queue_size=1) self.tf_buffer = tf2_ros.Buffer() self.tf_listner = tf2_ros.TransformListener(self.tf_buffer) self.fromMetric_sub = rospy.Subscriber('/pointsFromMetric', GetPointsFromMetric, self.publish_metric_to_rviz) self.transform_metric_pub = rospy.Publisher( '/rviz_pointsFromMetric', visualization_msgs.msg.Marker, queue_size=10)
def __init__(self): self._intialized = False # Set up tf buffer and listener. self._tf_buffer = tf2_ros.Buffer() self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
def __init__(self, context): super(RosTfTree, self).__init__(context) self.initialized = False self.setObjectName('RosTfTree') self._current_dotcode = None self._widget = QWidget() # factory builds generic dotcode items self.dotcode_factory = PydotFactory() # self.dotcode_factory = PygraphvizFactory() # generator builds rosgraph self.dotcode_generator = RosTfTreeDotcodeGenerator() self.tf2_buffer_ = tf2_ros.Buffer() self.tf2_listener_ = tf2_ros.TransformListener(self.tf2_buffer_) # dot_to_qt transforms into Qt elements using dot layout self.dot_to_qt = DotToQtGenerator() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_tf_tree'), 'resource', 'RosTfTree.ui') loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView}) self._widget.setObjectName('RosTfTreeUi') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) self._scene = QGraphicsScene() self._scene.setBackgroundBrush(Qt.white) self._widget.graphics_view.setScene(self._scene) self._widget.clear_buffer_push_button.setIcon( QIcon.fromTheme('edit-delete')) self._widget.clear_buffer_push_button.pressed.connect( self._clear_buffer) self._widget.refresh_graph_push_button.setIcon( QIcon.fromTheme('view-refresh')) self._widget.refresh_graph_push_button.pressed.connect( self._update_tf_graph) self._widget.highlight_connections_check_box.toggled.connect( self._redraw_graph_view) self._widget.auto_fit_graph_check_box.toggled.connect( self._redraw_graph_view) self._widget.fit_in_view_push_button.setIcon( QIcon.fromTheme('zoom-original')) self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view) self._widget.load_dot_push_button.setIcon( QIcon.fromTheme('document-open')) self._widget.load_dot_push_button.pressed.connect(self._load_dot) self._widget.save_dot_push_button.setIcon( QIcon.fromTheme('document-save-as')) self._widget.save_dot_push_button.pressed.connect(self._save_dot) self._widget.save_as_svg_push_button.setIcon( QIcon.fromTheme('document-save-as')) self._widget.save_as_svg_push_button.pressed.connect(self._save_svg) self._widget.save_as_image_push_button.setIcon( QIcon.fromTheme('image-x-generic')) self._widget.save_as_image_push_button.pressed.connect( self._save_image) self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection) self._deferred_fit_in_view.emit() context.add_widget(self._widget) self._force_refresh = False
def __init__(self): # Adjustable Parameters self.path_directory = rospy.get_param("~path_directory") self.mission_sound = rospy.get_param("~mission_sound") self.default_sound = rospy.get_param("~default_sound") self.battery_threshold = int(rospy.get_param("~battery_threshold", 50)) #[%] # Define Paths' File Name self.path_dict = ["HOME_A", "HOME_B", "HOME_1", "HOME_2", "HOME_3", "HOME_4", "HOME_5", "A_1", "A_2", "A_3", "A_4", "A_5", "B_1", "B_2", "B_3", "B_4", "B_5", "1_A", "2_A", "3_A", "4_A", "5_A", "1_B", "2_B", "3_B", "4_B", "5_B", "A_HOME", "B_HOME", "1_HOME", "2_HOME", "3_HOME", "4_HOME", "5_HOME", "HOME_CHARGE", "CHARGE_HOME"] # self.region_1 = Polygon(Point32(-2,2,0), Point32(10,2,0), Point32(10,-10,0), Point32(5,-10,0), Point32(5,-2,0), Point32(-2,-2,0)) # self.region_2 = Polygon(Point32(1,-2,0), Point32(1,-10,0), Point32(5,-10,0), Point32(5,-2,0)) # self.region_3 = Polygon(Point32(-2,-2,0), Point32(), Point32(), Point32()) # self.location_dict = {"store": self.region_1, "outdoor": self.region_2, "workshop": self.region_3} # Define Actions Type self.action_dict = {0:"None", 1:"Table_Picking", 2:"Table_Dropping"} # Internal USE Variables - Modify with consultation self.rate = rospy.Rate(5) # 5 [hz] <--> 0.2 [sec] self.tf2Buffer = tf2_ros.Buffer() self.tf2Listener = tf2_ros.TransformListener(self.tf2Buffer) # - Charging self.battery_level = 100 self.battery_safe = True self.battery_full = False self.go_charge = False self.start_charge = False self.go_home = False # - Point to Point self.route_list = [] self.route_seq = 0 self.action_list = [] self.action_seq = 0 self.waypoint_list = [] self.waypoint_seq = 0 self.delivery_status = "" self.action_status = True self.last_msg = None self.route_once = True self.XYZ = Point32() self.location = "HOME" self.speed = 0 self.delivery_id = "0" self.delivery_mission = "" self.mission_activity = "NO ACTION" self.fsm_state = "STANDBY" # Publisher self.all_pub = rospy.Publisher("/web/all_status", String, queue_size=1) # Subscribers self.battery_sub = rospy.Subscriber("/battery/percent", String, self.batteryCB, queue_size=1) self.mission_sub = rospy.Subscriber("/web/mission", String, self.missionCB, queue_size=1) self.fsm_sub = rospy.Subscriber("/fsm_node/state", FSMState, self.fsmCB, queue_size=1) self.odom_sub = rospy.Subscriber("/gyro/odom", Odometry, self.odomCB, queue_size=1) # Service Servers self.route_done_srv = rospy.Service("/route/done", Empty, self.route_doneSRV) self.action_done_srv = rospy.Service("/action/done", Empty, self.action_doneSRV) self.charging_done_srv = rospy.Service("/charging/done", Empty, self.charging_doneSRV) self.pick_table_done_srv = rospy.Service("/pick_table/done", Empty, self.action_doneSRV) self.drop_table_done_srv = rospy.Service("/drop_table/done", Empty, self.action_doneSRV) # Service Clients self.path_call = rospy.ServiceProxy("/change_path", ChangePath) self.charging_call = rospy.ServiceProxy("charging/call", Empty) self.pick_table_call = rospy.ServiceProxy("/pick_table/call", Empty) self.drop_table_call = rospy.ServiceProxy("/drop_table/call", Empty) self.rosbag_start_call = rospy.ServiceProxy("/rosbag/start", SetFileName) # self.rosbag_start_call = rospy.ServiceProxy("/rosbag/start", Empty) self.rosbag_stop_call = rospy.ServiceProxy("/rosbag/stop", Empty) self.fsm_call = rospy.ServiceProxy("/fsm_node/set_state", SetFSMState) self.sound_call = rospy.ServiceProxy("/sound/call", SetFileLocation) # Startup self.sound_call(self.default_sound) # Main Loop() self.main_loop()
def __init__(self, folda_name, category): """ fold_name is folda directory : str category is object type , directory names : list """ self.st = [] self.buf = tf2_ros.Buffer() self.lis = tf2_ros.TransformListener(self.buf) self._broadcaster = tf2_ros.TransformBroadcaster() self.file = folda_name self.category = category self.powers = manager.Manger(self.file, self.category) self.actions = {} self.trans = {} self.bos = {} self.eos = {} for c in self.category: name = self.file + "/" + c + "/" + FILE sname = self.file + "/" + c + "/" + SIGMA_FILE cor = np.loadtxt(self.file + "/" + c + "/" + COR) n = len(cor) cdic = {} for i in range(n): dic = {} data = np.loadtxt(name.format(i), delimiter=",") sdata = np.loadtxt(sname.format(i), delimiter=",") dic["gp"] = data dic["sig"] = sdata dic["cor"] = cor[i] cdic[i] = dic self.actions[c] = cdic trans = np.load(self.file + "/" + c + "/" + "trans.npy") bos = np.load(self.file + "/" + c + "/" + "trans_bos.npy") # eos = np.load(self.file + "/" + c + "/" +"trans_eos.npy") sl = 0 sts = [] while not rospy.is_shutdown(): try: st = np.loadtxt(self.file + "/" + c + "/" + SLENS.format(sl)) sts.append(st) sl += 1 except: break alst = np.zeros(len(cor)) enst = np.zeros(len(cor)) for i in range(len(sts)): st = sts[i] if len(st.shape) == 2: for s in st: j = int(s[0]) alst[j] += 1 else: j = int(st[0]) alst[j] += 1 enst[j] += 1 eos = np.zeros(len(cor)) for i in range(len(cor)): eos[i] = enst[i] / alst[i] * (1.0 - 2 * E) + E self.trans[c] = trans self.eos[c] = eos self.bos[c] = bos self.cord = {} self.back_action = None self.gps = [Calc() for i in range(3)] rospy.Subscriber("/tf_pulse", Empty, self.broadcast, queue_size=10)
def main(): """ Main Script """ while not rospy.is_shutdown(): planner = PathPlanner("right_arm") limb = Limb("right") joint_angles = limb.joint_angles() print(joint_angles) camera_angle = { 'right_j6': 1.72249609375, 'right_j5': 0.31765625, 'right_j4': -0.069416015625, 'right_j3': 1.1111962890625, 'right_j2': 0.0664150390625, 'right_j1': -1.357775390625, 'right_j0': -0.0880478515625 } limb.set_joint_positions(camera_angle) limb.move_to_joint_positions(camera_angle, timeout=15.0, threshold=0.008726646, test=None) #drawing_angles = {'right_j6': -2.00561328125, 'right_j5': -1.9730205078125, 'right_j4': 1.5130146484375, 'right_j3': -1.0272568359375, 'right_j2': 1.24968359375, 'right_j1': -0.239498046875, 'right_j0': 0.4667275390625} #print(joint_angles) #drawing_angles = {'right_j6': -1.0133310546875, 'right_j5': -1.5432158203125, 'right_j4': 1.4599892578125, 'right_j3': -0.04361328125, 'right_j2': 1.6773486328125, 'right_j1': 0.019876953125, 'right_j0': 0.4214736328125} drawing_angles = { 'right_j6': 1.9668515625, 'right_j5': 1.07505859375, 'right_j4': -0.2511611328125, 'right_j3': 0.782650390625, 'right_j2': 0.25496875, 'right_j1': -0.3268349609375, 'right_j0': 0.201146484375 } raw_input("Press <Enter> to take picture: ") waypoints_abstract = vision() print(waypoints_abstract) #ar coordinate : x = 0.461067 y = -0.235305 #first get the solution of the maze #solutionpoints = [(0,0),(-0.66,0.16), (-0.7, 0.4)] # Make sure that you've looked at and understand path_planner.py before starting tfBuffer = tf2_ros.Buffer() tfListener = tf2_ros.TransformListener(tfBuffer) r = rospy.Rate(10) #find trans from while not rospy.is_shutdown(): try: trans = tfBuffer.lookup_transform('base', 'ar_marker_0', rospy.Time()).transform break except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): if tf2_ros.LookupException: print("lookup") elif tf2_ros.ConnectivityException: print("connect") elif tf2_ros.ExtrapolationException: print("extra") # print("not found") pass r.sleep() mat = as_transformation_matrix(trans) point_spaces = [] for point in waypoints_abstract: # for point in solutionpoints: point = np.array([point[0], point[1], 0, 1]) point_space = np.dot(mat, point) point_spaces.append(point_space) print(point_spaces) length_of_points = len(point_spaces) raw_input("Press <Enter> to move the right arm to drawing position: ") limb.set_joint_positions(drawing_angles) limb.move_to_joint_positions(drawing_angles, timeout=15.0, threshold=0.008726646, test=None) ## ## Add the obstacle to the planning scene here ## #add obstacle size = [0.78, 1.0, 0.05] name = "box" pose = PoseStamped() pose.header.frame_id = "base" pose.pose.position.x = 0.77 pose.pose.position.y = 0.0 pose.pose.position.z = -0.18 #Orientation as a quaternion pose.pose.orientation.x = 0.0 pose.pose.orientation.y = 0.0 pose.pose.orientation.z = 0.0 pose.pose.orientation.w = 1.0 planner.add_box_obstacle(size, name, pose) #limb.set_joint_positions( drawing_angles) #limb.move_to_joint_positions( drawing_angles, timeout=15.0, threshold=0.008726646, test=None) #starting position x, y, z = 0.67, 0.31, -0.107343 goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 while not rospy.is_shutdown(): try: waypoint = [] for point in point_spaces: goal_1.pose.position.x = point[0] goal_1.pose.position.y = point[1] goal_1.pose.position.z = -0.113343 #set this value when put different marker waypoint.append(copy.deepcopy(goal_1.pose)) plan, fraction = planner.plan_straight(waypoint, []) print(fraction) raw_input("Press <Enter> to move the right arm to draw: ") if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e traceback.print_exc() else: break raw_input("Press <Enter> to start again: ")
def controller(robot_frame, target_frame): """ Controls a robot whose position is denoted by robot_frame, to go to a position denoted by target_frame Inputs: - robot_frame: the tf frame of the robot base. - target_frame: the tf frame of the desired position. """ ################################### YOUR CODE HERE ############## #Create a publisher and a tf buffer, which is primed with a tf listener #TODO: replace 'INPUT TOPIC' with the correct name for the ROS topic on which # the robot accepts velocity inputs. tfBuffer = tf2_ros.Buffer() tfListener = tf2_ros.TransformListener(tfBuffer) pub = rospy.Publisher(robot_frame + '/cmd_vel', Twist, queue_size=10) # Create a timer object that will sleep long enough to result in # a 10Hz publishing rate r = rospy.Rate(10) # 10hz K1 = .3 K2 = 6 K1d = .1 K2d = .1 # Loop until the node is killed with Ctrl-C t_last = rospy.get_time() last_translation_x_error = 0 last_rotation_error = 0 keepPath = True while not rospy.is_shutdown(): #TODO: Replace 'SOURCE FRAME' and 'TARGET FRAME' with the appropriate TF frame names. #trans = tfBuffer.lookup_transform('SOURCE FRAME', 'TARGET FRAME', rospy.Time()) try: trans = tfBuffer.lookup_transform( robot_frame, target_frame, rospy.Time()) ##MAKE CHANGES HERE TO ARGUMENTS # Process trans to get your state error # Generate a control command to send to the robot translation_x_error = trans.transform.translation.x rotation_error = trans.transform.translation.y ''' t_current = rospy.get_time() dt = t_current - t_last t_last = t_current translation_x_error_der = (translation_x_error - last_translation_x_error)/dt rotation_error_der = (rotation_error - last_rotation_error)/dt print(translation_x_error_der, rotation_error_der) print('----------') ''' last_translation_x_error = translation_x_error last_rotation_error = rotation_error if np.sqrt( abs(trans.transform.translation.x)**2 + abs(trans.transform.translation.y)**2) < .4: K2 = 6 publish_task_update(robot_frame, False, True) control_command = Twist() control_command.linear.x = 0 control_command.angular.z = 0 pub.publish(control_command) continue ### Publish to 'robot_name/task' with need_path_update! control_command = Twist() max_rotation_speed = .2 max_translation_speed = 1 if K2 > .1: if np.sqrt( abs(trans.transform.translation.x)**2 + abs(trans.transform.translation.y)**2) > 3: K2 = K2 * .95 else: K2 = K2 * .98 if abs(rotation_error) > .1: if abs(rotation_error * -K2) > max_rotation_speed: control_command.angular.z = max_rotation_speed * ( -rotation_error) / abs(rotation_error) else: control_command.angular.z = rotation_error * -K2 else: if abs(rotation_error * -K2) > max_rotation_speed: control_command.angular.z = max_translation_speed * ( -rotation_error) / abs(rotation_error) else: control_command.angular.z = rotation_error * -K2 if abs(translation_x_error * K1) > max_translation_speed: control_command.linear.x = max_translation_speed * translation_x_error / abs( translation_x_error) else: control_command.linear.x = translation_x_error * K1 ''' try: if (translation_x_error > .2): print(robot_name) ##if (r != inf): msg = rospy.wait_for_message("/" + robot_name + "/r", Float32, 100) print('got an r message at the very least') print(msg) if (msg.data < .2): print('published task') publish_task_update(robot_frame, True, False) ## Needs new a star except Exception as e: print('Timeout waiting for robot_name/r') ''' ''' control_command.linear.x = translation_x_error * K1 + translation_x_error_der * K1d control_command.angular.z = rotation_error * -K2 + rotation_error_der *K2d ''' #TODO: Generate this #################################### end your code ############### pub.publish(control_command) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: pass # Use our rate object to sleep until it is time to publish again r.sleep()
def __init__(self): # self.br = tf2_ros.TransformBroadcaster() self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) rospy.wait_for_service('add_compound') self.add_compound = rospy.ServiceProxy('add_compound', AddCompound) self.spawn_frame = rospy.get_param("~frame", "spawn_frame") self.ts = TransformStamped() self.ts.header.frame_id = "map" self.ts.child_frame_id = self.spawn_frame self.ts.transform.rotation.w = 1.0 self.timer = rospy.Timer(rospy.Duration(0.1), self.update) self.count = 0 self.server = InteractiveMarkerServer("body_spawn") self.im = InteractiveMarker() self.im.header.frame_id = self.spawn_frame self.im.name = "body_spawner" self.im.description = "Spawn a new body" self.menu_handler = MenuHandler() # TODO(lucasw) click and drag resizing will be best served # by and interactive marker child that has a frame tf defined # by the parent. menu = InteractiveMarkerControl() menu.interaction_mode = InteractiveMarkerControl.MENU menu.description = "spawn" menu.name = "spawn_menu" box = Marker() box.header.frame_id = self.spawn_frame box.type = Marker.CUBE box.scale.x = 0.5 box.scale.y = 0.5 box.scale.z = 0.5 box.color.r = 0.8 box.color.g = 0.8 box.color.b = 0.8 box.color.a = 1.0 box.frame_locked = True menu.markers.append(box) self.im.controls.append(menu) self.menu_handler.insert("spawn", callback=self.process_feedback) # TODO(lucasw) have a submenu that is updated with a list # of all tf frames, and selecting one will cause that tf to # be the parent of this marker. self.server.insert(self.im, self.process_feedback) self.server.applyChanges() # TODO(lucasw) what does this do? self.menu_handler.apply(self.server, self.im.name) # resize x self.resize_x_server = InteractiveMarkerServer("body_spawn/resize_x") self.resize_x_im = InteractiveMarker() self.resize_x_im.header.frame_id = self.spawn_frame self.resize_x_im.name = "body_spawner_resize_x" self.resize_x_im.description = "resize x of new body" self.resize_x = InteractiveMarkerControl() self.resize_x.interaction_mode = InteractiveMarkerControl.MOVE_AXIS self.resize_x.name = "resize_x" self.resize_x.orientation.w = 1 self.resize_x.orientation.x = 1 self.resize_x.orientation.y = 0 self.resize_x.orientation.z = 0 arrow = Marker() arrow.type = Marker.ARROW # arrow.pose.position.x = box.scale.x * 0.5 arrow.scale.x = 0.8 arrow.scale.y = 0.3 arrow.scale.z = 0.3 arrow.color.r = 1.0 arrow.color.g = 0.2 arrow.color.b = 0.15 arrow.color.a = 1.0 arrow.frame_locked = True self.resize_x.markers.append(arrow) self.resize_x_im.controls.append(self.resize_x) self.resize_x_server.insert(self.resize_x_im, self.process_resize_feedback) self.resize_x_server.applyChanges() # resize y self.resize_y_server = InteractiveMarkerServer("body_spawn/resize_y") self.resize_y_im = InteractiveMarker() self.resize_y_im.header.frame_id = self.spawn_frame self.resize_y_im.name = "body_spawner_resize_y" self.resize_y_im.description = "resize y of new body" self.resize_y = InteractiveMarkerControl() self.resize_y.interaction_mode = InteractiveMarkerControl.MOVE_AXIS self.resize_y.name = "resize_y" self.resize_y.orientation.w = 1 self.resize_y.orientation.x = 0 self.resize_y.orientation.y = 0 self.resize_y.orientation.z = 1 arrow_y = copy.deepcopy(arrow) arrow_y.color.r = 0.2 arrow_y.color.g = 1.0 arrow_y.color.b = 0.15 arrow_y.pose.orientation = self.resize_y.orientation self.resize_y.markers.append(arrow_y) self.resize_y_im.controls.append(self.resize_y) self.resize_y_server.insert(self.resize_y_im, self.process_resize_feedback) self.resize_y_server.applyChanges() # resize y self.resize_z_server = InteractiveMarkerServer("body_spawn/resize_z") self.resize_z_im = InteractiveMarker() self.resize_z_im.header.frame_id = self.spawn_frame self.resize_z_im.name = "body_spawner_resize_z" self.resize_z_im.description = "resize z of new body" self.resize_z = InteractiveMarkerControl() self.resize_z.interaction_mode = InteractiveMarkerControl.MOVE_AXIS self.resize_z.name = "resize_z" self.resize_z.orientation.w = 1 self.resize_z.orientation.x = 0 self.resize_z.orientation.y = 1 self.resize_z.orientation.z = 0 arrow_z = copy.deepcopy(arrow) arrow_z.color.r = 0.2 arrow_z.color.g = 0.04 arrow_z.color.b = 1.0 arrow_z.pose.orientation = self.resize_z.orientation self.resize_z.markers.append(arrow_z) self.resize_z_im.controls.append(self.resize_z) self.resize_z_server.insert(self.resize_z_im, self.process_resize_feedback) self.resize_z_server.applyChanges() # add linear impulse self.linear_vel_server = InteractiveMarkerServer("body_spawn/linear_vel") self.linear_vel_im = InteractiveMarker() self.linear_vel_im.header.frame_id = self.spawn_frame self.linear_vel_im.name = "body_spawner_linear_vel" self.linear_vel_im.description = "add impulse to body" self.linear_vel_pt = [0, 0, 0, 0] self.linear_vel_control = InteractiveMarkerControl() self.linear_vel_control.interaction_mode = InteractiveMarkerControl.MOVE_3D self.linear_vel_control.name = "linear_vel" self.linear_vel_control.orientation.w = 1 self.linear_vel_control.orientation.x = 0 self.linear_vel_control.orientation.y = 1 self.linear_vel_control.orientation.z = 0 box = Marker() box.header.frame_id = self.spawn_frame box.type = Marker.CUBE box.scale.x = 0.1 box.scale.y = 0.1 box.scale.z = 0.1 box.color.r = 0.8 box.color.g = 0.8 box.color.b = 0.2 box.color.a = 1.0 box.frame_locked = True self.linear_vel_control.markers.append(box) self.linear_vel_im.controls.append(self.linear_vel_control) self.linear_vel_server.insert(self.linear_vel_im, self.process_vel_feedback) self.linear_vel_server.applyChanges() self.marker_pub = rospy.Publisher("/body_spawn/misc_markers", Marker, queue_size=1) self.linear_vel_marker = Marker() self.linear_vel_marker.header.frame_id = self.spawn_frame self.linear_vel_marker.type = Marker.LINE_LIST self.linear_vel_marker.scale.x = 0.07 self.linear_vel_marker.scale.y = 0.07 self.linear_vel_marker.scale.z = 0.07 self.linear_vel_marker.color.r = 0.8 self.linear_vel_marker.color.g = 0.8 self.linear_vel_marker.color.b = 0.2 self.linear_vel_marker.color.a = 1.0 self.linear_vel_marker.frame_locked = True pt = Point() self.linear_vel_marker.points.append(pt) pt = Point() self.linear_vel_marker.points.append(pt) self.marker_pub.publish(self.linear_vel_marker)
import rospy import time import tf2_ros as tf from tf_class import Landmarks_TF from apriltag_ros.msg import AprilTagDetectionArray if __name__ == "__main__": rospy.init_node("tag_tf_broadcaster", anonymous=True) rate = rospy.Rate(100) buf = tf.Buffer() ls = tf.TransformListener(buf) #gt = 'config/optimized_pose/left_side_optim2.txt' gt = 'config/left_side_gmap_gt.txt' ldmk = Landmarks_TF(buf, ls, gt_file=gt, suffix='_pc', cur_frame='odom') tag_sub = rospy.Subscriber("/tag_detections", AprilTagDetectionArray, ldmk.cam_init) while not rospy.is_shutdown(): if ldmk.can_update: ldmk.publish_dynamic() #Publish dynamic TF rate.sleep()
def __init__(self): rospy.init_node("bitbots_transformer") self.tf_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(10.0)) self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) # time 0 takes the most current transform available self.tf_buffer.can_transform("base_footprint", "camera_optical_frame", rospy.Time(0), timeout=rospy.Duration(30)) rospy.Subscriber(rospy.get_param("~ball/ball_topic", "/ball_in_image"), BallsInImage, self._callback_ball, queue_size=1) if rospy.get_param("~lines/lines_relative", True): rospy.Subscriber(rospy.get_param("~lines/lines_topic", "/line_in_image"), LineInformationInImage, self._callback_lines, queue_size=1) if rospy.get_param("~lines/pointcloud", False): rospy.Subscriber(rospy.get_param("~lines/lines_topic", "/line_in_image"), LineInformationInImage, self._callback_lines_pc, queue_size=1) rospy.Subscriber(rospy.get_param("~goals/goals_topic", "/goal_in_image"), GoalInImage, self._callback_goal, queue_size=1) rospy.Subscriber(rospy.get_param("~obstacles/obstacles_topic", "/obstacles_in_image"), ObstaclesInImage, self._callback_obstacles, queue_size=1) rospy.Subscriber(rospy.get_param("~camera_info/camera_info_topic", "/camera_info"), CameraInfo, self._callback_camera_info, queue_size=1) self.marker_pub = rospy.Publisher("ballpoint", Marker, queue_size=1) self.ball_relative_pub = rospy.Publisher("ball_relative", BallRelative, queue_size=1) if rospy.get_param("~lines/lines_relative", True): self.line_relative_pub = rospy.Publisher("line_relative", LineInformationRelative, queue_size=1) if rospy.get_param("~lines/pointcloud", True): self.line_relative_pc_pub = rospy.Publisher("line_relative_pc", PointCloud2, queue_size=1) self.goal_relative_pub = rospy.Publisher("goal_relative", GoalRelative, queue_size=1) self.obstacle_relative_pub = rospy.Publisher("obstacles_relative", ObstaclesRelative, queue_size=1) self.camera_info = None self.ball_height = rospy.get_param("~ball/ball_radius", 0.075) self.publish_frame = "base_footprint" rospy.spin()
def __init__(self): self.eye_on_hand = rospy.get_param('eye_on_hand', False) """ if false, it is a eye-on-base calibration :type: bool """ # tf names self.robot_effector_frame = rospy.get_param('robot_effector_frame', 'tool0') """ robot tool tf name :type: string """ self.robot_base_frame = rospy.get_param('robot_base_frame', 'base_link') """ robot base tf name :type: str """ self.tracking_base_frame = rospy.get_param('tracking_base_frame', 'optical_origin') """ tracking system tf name :type: str """ self.tracking_marker_frame = rospy.get_param('tracking_marker_frame', 'optical_target') """ tracked object tf name :type: str """ # tf structures self.tfBuffer = tf2_ros.Buffer() """ used to get transforms to build each sample :type: tf2_ros.Buffer """ self.tfListener = tf2_ros.TransformListener(self.tfBuffer) """ used to get transforms to build each sample :type: tf2_ros.TransformListener """ self.tfBroadcaster = tf2_ros.TransformBroadcaster() """ used to publish the calibration after saving it :type: tf.TransformBroadcaster """ # internal input data self.samples = [] """ list of acquired samples Each sample is a dictionary going from 'rob' and 'opt' to the relative sampled transform in tf tuple format. :type: list[dict[str, ((float, float, float), (float, float, float, float))]] """ # calibration service rospy.wait_for_service('compute_effector_camera_quick') self.calibrate = rospy.ServiceProxy('compute_effector_camera_quick', compute_effector_camera_quick) """
def __init__(self): self.use_most_recent_tf = False # use most recent tf time self.buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.buffer)
def __init__(self, blackboard, node: Node): self.node = node self._blackboard = blackboard self.body_config = self.node.get_parameter( "behavior/body").get_parameter_value().string_value # This pose is not supposed to be used as robot pose. Just as precision measurement for the TF position. self.pose = PoseWithCovarianceStamped() self.tf_buffer = tf2.Buffer(cache_time=Duration(seconds=30)) self.tf_listener = tf2.TransformListener(self.tf_buffer) self.odom_frame = self.node.get_parameter( '~odom_frame').get_parameter_value().string_value self.map_frame = self.node.get_parameter( '~map_frame').get_parameter_value().string_value self.ball_frame = self.node.get_parameter( '~ball_frame').get_parameter_value().string_value self.base_footprint_frame = self.node.get_parameter( '~base_footprint_frame').get_parameter_value().string_value self.ball = PointStamped() # The ball in the base footprint frame self.ball_odom = PointStamped( ) # The ball in the odom frame (when localization is not usable) self.ball_odom.header.stamp = rclpy.Time(seconds=0, nanoseconds=0) self.ball_odom.header.frame_id = self.odom_frame self.ball_map = PointStamped( ) # The ball in the map frame (when localization is usable) self.ball_map.header.stamp = rclpy.Time(seconds=0, nanoseconds=0) self.ball_map.header.frame_id = self.map_frame self.ball_teammate = PointStamped() self.ball_teammate.header.stamp = rclpy.Time(seconds=0, nanoseconds=0) self.ball_teammate.header.frame_id = self.map_frame self.ball_lost_time = Duration(seconds=self.node.get_parameter( 'behavior/body/ball_lost_time').get_parameter_value().double_value) self.ball_twist_map = None self.ball_filtered = None self.ball_twist_lost_time = Duration(seconds=self.node.get_parameter( 'behavior/body/ball_twist_lost_time').get_parameter_value(). double_value) self.ball_twist_precision_threshold = self.node.get_parameter( 'behavior/body/ball_twist_precision_threshold' ).get_parameter_value().double_value self.reset_ball_filter = self.node.create_client( Trigger, 'ball_filter_reset') self.goal = GoalRelative() # The goal in the base footprint frame self.goal_odom = GoalRelative() self.goal_odom.header.stamp = self.get_clock().now() self.goal_odom.header.frame_id = self.odom_frame self.my_data = dict() self.counter = 0 self.ball_seen_time = rclpy.Time(seconds=0, nanoseconds=0) self.ball_seen_time_teammate = rclpy.Time(seconds=0, nanoseconds=0) self.goal_seen_time = rclpy.Time(seconds=0, nanoseconds=0) self.ball_seen = False self.ball_seen_teammate = False self.field_length = self.node.get_parameter( 'field_length').get_parameter_value().double_value self.field_width = self.node.get_parameter( 'field_width').get_parameter_value().double_value self.goal_width = self.node.get_parameter( 'goal_width').get_parameter_value().double_value self.map_margin = self.node.get_parameter( 'behavior/body/map_margin').get_parameter_value().double_value self.obstacle_costmap_smoothing_sigma = self.node.get_parameter( '"behavior/body/obstacle_costmap_smoothing_sigma"' ).get_parameter_value().double_value self.obstacle_cost = self.node.get_parameter( 'behavior/body/obstacle_cost').get_parameter_value().double_value self.use_localization = self.node.get_parameter( 'behavior/body/use_localization').get_parameter_value( ).double_value self.pose_precision_threshold = self.node.get_parameter( 'behavior/body/pose_precision_threshold').get_parameter_value( ).double_value # Publisher for visualization in RViZ self.ball_publisher = self.create_publisher(PointStamped, 'debug/viz_ball', 1) self.goal_publisher = self.create_publisher(PoseWithCertaintyArray, 'debug/viz_goal', 1) self.ball_twist_publisher = self.create_publisher( TwistStamped, 'debug/ball_twist', 1) self.used_ball_pub = self.create_publisher(PointStamped, 'debug/used_ball', 1) self.which_ball_pub = self.create_publisher( Header, 'debug/which_ball_is_used', 1) self.costmap_publisher = self.create_publisher(OccupancyGrid, 'debug/costmap', 1) self.base_costmap = None # generated once in constructor field features self.costmap = None # updated on the fly based on the base_costmap self.gradient_map = None # global heading map (static) only dependent on field structure # Calculates the base costmap and gradient map based on it self.calc_base_costmap() self.calc_gradients()
def __init__(self): # Read necessary parameters self.pc = rospy.get_param('~particle_count', 10) # Particle Count self.map_frame = rospy.get_param('~map_frame', 'map') # map frame_id self.mbes_frame = rospy.get_param('~mbes_link', 'mbes_link') # mbes frame_id odom_frame = rospy.get_param('~odom_frame', 'odom') meas_model_as = rospy.get_param('~mbes_as', '/mbes_sim_server') # map frame_id self.beams_num = rospy.get_param("~num_beams_sim", 20) self.beams_real = rospy.get_param("~n_beams_mbes", 512) self.mbes_angle = rospy.get_param("~mbes_open_angle", np.pi / 180. * 60.) # Initialize tf listener tfBuffer = tf2_ros.Buffer() tf2_ros.TransformListener(tfBuffer) try: rospy.loginfo("Waiting for transforms") mbes_tf = tfBuffer.lookup_transform('hugin/base_link', 'hugin/mbes_link', rospy.Time(0), rospy.Duration(20)) self.base2mbes_mat = matrix_from_tf(mbes_tf) m2o_tf = tfBuffer.lookup_transform(self.map_frame, odom_frame, rospy.Time(0), rospy.Duration(20)) self.m2o_mat = matrix_from_tf(m2o_tf) rospy.loginfo("Transforms locked - pf node") except: rospy.loginfo( "ERROR: Could not lookup transform from base_link to mbes_link" ) # Read covariance values meas_cov = float(rospy.get_param('~measurement_covariance', 0.01)) cov_string = rospy.get_param('~motion_covariance') cov_string = cov_string.replace('[', '') cov_string = cov_string.replace(']', '') cov_list = list(cov_string.split(", ")) motion_cov = list(map(float, cov_list)) cov_string = rospy.get_param('~init_covariance') cov_string = cov_string.replace('[', '') cov_string = cov_string.replace(']', '') cov_list = list(cov_string.split(", ")) init_cov = list(map(float, cov_list)) cov_string = rospy.get_param('~resampling_noise_covariance') cov_string = cov_string.replace('[', '') cov_string = cov_string.replace(']', '') cov_list = list(cov_string.split(", ")) self.res_noise_cov = list(map(float, cov_list)) # Initialize list of particles self.particles = np.empty(self.pc, dtype=object) for i in range(self.pc): self.particles[i] = Particle(self.beams_num, self.pc, i, self.base2mbes_mat, self.m2o_mat, init_cov=init_cov, meas_cov=meas_cov, process_cov=motion_cov) self.time = None self.old_time = None self.pred_odom = None self.latest_mbes = PointCloud2() self.prev_mbes = PointCloud2() self.poses = PoseArray() self.poses.header.frame_id = odom_frame self.avg_pose = PoseWithCovarianceStamped() self.avg_pose.header.frame_id = odom_frame # Initialize particle poses publisher pose_array_top = rospy.get_param("~particle_poses_topic", '/particle_poses') self.pf_pub = rospy.Publisher(pose_array_top, PoseArray, queue_size=10) # Initialize average of poses publisher avg_pose_top = rospy.get_param("~average_pose_topic", '/average_pose') self.avg_pub = rospy.Publisher(avg_pose_top, PoseWithCovarianceStamped, queue_size=10) # Establish subscription to odometry message (intentionally last) odom_top = rospy.get_param("~odometry_topic", 'odom') rospy.Subscriber(odom_top, Odometry, self.odom_callback) # Expected meas of PF outcome at every time step pf_mbes_top = rospy.get_param("~average_mbes_topic", '/avg_mbes') self.pf_mbes_pub = rospy.Publisher(pf_mbes_top, PointCloud2, queue_size=1) self.stats = rospy.Publisher('/pf/n_eff', Float32, queue_size=10) rospy.loginfo("Particle filter class successfully created") mbes_pc_top = rospy.get_param("~particle_sim_mbes_topic", '/sim_mbes') self.pcloud_pub = rospy.Publisher(mbes_pc_top, PointCloud2, queue_size=10) # Load mesh print("Loading data") svp_path = rospy.get_param('~sound_velocity_prof') mesh_path = rospy.get_param('~mesh_path') sound_speeds = csv_data.csv_asvp_sound_speed.parse_file(svp_path) data = np.load(mesh_path) V, F, bounds = data['V'], data['F'], data['bounds'] print("Mesh loaded") # Create draper self.draper = base_draper.BaseDraper(V, F, bounds, sound_speeds) self.draper.set_ray_tracing_enabled(False) print("draper created") # Action server for MBES pings sim (necessary to be able to use UFO maps as well) self.as_ping = actionlib.SimpleActionServer('/mbes_sim_server', MbesSimAction, execute_cb=self.mbes_as_cb) # Subscription to real mbes pings mbes_pings_top = rospy.get_param("~mbes_pings_topic", 'mbes_pings') rospy.Subscriber(mbes_pings_top, PointCloud2, self.mbes_real_cb) # Create expected MBES beams directions angle_step = self.mbes_angle / self.beams_num self.beams_dir = [] for i in range(0, self.beams_num): roll_step = rotation_matrix(-self.mbes_angle / 2. + angle_step * i, (1, 0, 0))[0:3, 0:3] rot = roll_step[:, 2] self.beams_dir.append(rot / np.linalg.norm(rot)) self.update_rviz() rospy.spin()
def mover(): global laser_range rospy.init_node('mover', anonymous=True) tfBuffer = tf2_ros.Buffer() tfListener = tf2_ros.TransformListener(tfBuffer) rospy.sleep(1.0) # subscribe to odometry data rospy.Subscriber('odom', Odometry, get_odom_dir) # subscribe to LaserScan data rospy.Subscriber('scan', LaserScan, get_laserscan) # subscribe to map occupancy data rospy.Subscriber('map', OccupancyGrid, callback, tfBuffer) rospy.on_shutdown(stopbot) rate = rospy.Rate(5) # 5 Hz # save start time to file start_time = time.time() # initialize variable to write elapsed time to file timeWritten = 0 # find direction with the largest distance from the Lidar, # rotate to that direction, and start moving rotatebot(0) rospy.loginfo(['len laser range', len(laser_range)]) rospy.loginfo(['finished turning 0']) pick_direction() while not rospy.is_shutdown(): if laser_range.size != 0: # check distances in front of TurtleBot and find values less # than stop_distance lri = (laser_range[front_angles] < float(stop_distance)).nonzero() rospy.loginfo('Distances: %s', str(lri)) else: lri[0] = [] # if the list is not empty if (len(lri[0]) > 0): rospy.loginfo(['Stop!']) stopbot() # find direction with the largest distance from the Lidar # rotate to that direction # start moving rospy.loginfo(['running our function']) pick_direction() # check if SLAM map is complete if timeWritten: if closure(occdata): # map is complete, so save current time into file with open("maptime.txt", "w") as f: f.write("Elapsed Time: " + str(time.time() - start_time)) timeWritten = 1 # play a sound # soundhandle = SoundClient() # rospy.sleep(1) # soundhandle.stopAll() # soundhandle.play(SoundRequest.NEEDS_UNPLUGGING) rospy.sleep(2) # save the map cv2.imwrite('mazemap.png', occdata) rate.sleep()
import tf.transformations as t from aruco_pose.msg import MarkerArray from mavros import mavlink # TODO: check attitude is present # TODO: disk free space # TODO: map, base_link, body # TODO: rc service # TODO: perform commander check, ekf2 status on PX4 # TODO: check if FCU params setter succeed # TODO: selfcheck ROS service (with blacklists for checks) rospy.init_node('selfcheck') tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) failures = [] infos = [] current_check = None def failure(text, *args): msg = text % args rospy.logwarn('%s: %s', current_check, msg) failures.append(msg) def info(text, *args): msg = text % args rospy.loginfo('%s: %s', current_check, msg)
def __init__(self): rospy.init_node("uw_teleop") self.vel_pub1 = rospy.Publisher('/dataNavigator1', Odometry, queue_size=10) self.vel_pub2 = rospy.Publisher('/dataNavigator2', Odometry, queue_size=10) self.rate = rospy.Rate(20) self.joy_sub = rospy.Subscriber("/joy", Joy, self.joy_msg_callback) self.bf1_odom_sub = rospy.Subscriber("/uwsim/BlueFox1Odom", Odometry, self.pose1_callback) self.bf2_odom_sub = rospy.Subscriber("/uwsim/BlueFox2Odom", Odometry, self.pose2_callback) ####### Make a lidar by stacking multibeam sensors ################## self.bf1_laser_sub = rospy.Subscriber("/BlueFox1/multibeam", LaserScan, self.laser_callback) self.bf1_laser1_sub = rospy.Subscriber("/BlueFox1/multibeam1", LaserScan, self.laser1_callback) self.bf1_laser2_sub = rospy.Subscriber("/BlueFox1/multibeam2", LaserScan, self.laser2_callback) self.bf1_laser3_sub = rospy.Subscriber("/BlueFox1/multibeam3", LaserScan, self.laser3_callback) self.bf1_laser4_sub = rospy.Subscriber("/BlueFox1/multibeam4", LaserScan, self.laser4_callback) self.bf1_laser5_sub = rospy.Subscriber("/BlueFox1/multibeam5", LaserScan, self.laser5_callback) self.bf1_laser6_sub = rospy.Subscriber("/BlueFox1/multibeam6", LaserScan, self.laser6_callback) self.bf1_laser7_sub = rospy.Subscriber("/BlueFox1/multibeam7", LaserScan, self.laser7_callback) self.bf1_laser8_sub = rospy.Subscriber("/BlueFox1/multibeam8", LaserScan, self.laser8_callback) self.bf1_laser9_sub = rospy.Subscriber("/BlueFox1/multibeam9", LaserScan, self.laser9_callback) self.bf1_laser10_sub = rospy.Subscriber("/BlueFox1/multibeam10", LaserScan, self.laser10_callback) self.bf1_laser11_sub = rospy.Subscriber("/BlueFox1/multibeam11", LaserScan, self.laser11_callback) self.laser_data = {} self.combined_laserscan = LaserScan() ####################################################################### self.bf2_laser_sub = rospy.Subscriber("/BlueFox2/multibeam", LaserScan, self.laser2_callback) # messages required by /ndt_mapping self.lidar_pc2_pub = rospy.Publisher('velodyne_points', PointCloud2, queue_size=10) self.imu_pub = rospy.Publisher("/imu_raw", Imu, queue_size=10) self.vehicle_odom_pub = rospy.Publisher('/odom_pose', Odometry, queue_size=10) self.joy_data = Joy() self.vel_cmd1 = Odometry() self.vel_cmd2 = Odometry() self.bf1_pose = None self.bf2_pose = None self.odom_hz = 10 self.bf1_vel = None self.bf2_vel = None self.bf1_laser = LaserScan() self.bf2_laser = LaserScan() self.joy_button = None self.lidar_counter = 0 # this is a hack to make lidar spin self.laser_projection = LaserProjection() self.velodyne_mapped_laserscan = LaserScan() self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
def __init__(self): rospy.init_node('turtlebot_mapping') ## Use simulation time (i.e. get time from rostopic /clock) rospy.set_param('use_sim_time', 'true') rate = rospy.Rate(10) while rospy.Time.now() == rospy.Time(0): rate.sleep() ## Get transformation of camera frame with respect to the base frame self.tfBuffer = tf2_ros.Buffer() self.tfListener = tf2_ros.TransformListener(self.tfBuffer) while True: try: # notably camera_link and not camera_depth_frame below, not sure why self.raw_base_to_camera = self.tfBuffer.lookup_transform( "base_footprint", "base_scan", rospy.Time()).transform break except tf2_ros.LookupException: rate.sleep() rotation = self.raw_base_to_camera.rotation translation = self.raw_base_to_camera.translation tf_theta = get_yaw_from_quaternion(rotation) self.base_to_camera = [translation.x, translation.y, tf_theta] ## Colocate the `ground_truth` and `base_footprint` frames for visualization purposes tf2_ros.StaticTransformBroadcaster().sendTransform( create_transform_msg((0, 0, 0), (0, 0, 0, 1), "ground_truth", "base_footprint")) ## Initial state for EKF self.EKF = None self.EKF_time = None self.current_control = np.zeros(2) self.latest_pose = None self.latest_pose_time = None self.controls = deque() self.scans = deque() N_map_lines = ArenaParams.shape[1] self.x0_map = ArenaParams.T.flatten() self.x0_map[4:] = self.x0_map[4:] + np.vstack(( NoiseParams["std_alpha"] * np.random.randn(N_map_lines - 2), # first two lines fixed NoiseParams["std_r"] * np.random.randn(N_map_lines - 2))).T.flatten() self.P0_map = np.diag( np.concatenate( (np.zeros(4), np.array([[ NoiseParams["std_alpha"]**2 for i in range(N_map_lines - 2) ], [NoiseParams["std_r"]**2 for i in range(N_map_lines - 2)]]).T.flatten()))) ## Set up publishers and subscribers self.tfBroadcaster = tf2_ros.TransformBroadcaster() rospy.Subscriber('/scan', LaserScan, self.scan_callback) rospy.Subscriber('/cmd_vel', Twist, self.control_callback) rospy.Subscriber('/gazebo/model_states', ModelStates, self.state_callback) self.ground_truth_ct = 0 self.ground_truth_map_pub = rospy.Publisher("ground_truth_map", Marker, queue_size=10) self.ground_truth_map_marker = Marker() self.ground_truth_map_marker.header.frame_id = "/world" self.ground_truth_map_marker.header.stamp = rospy.Time.now() self.ground_truth_map_marker.ns = "ground_truth" self.ground_truth_map_marker.type = 5 # line list self.ground_truth_map_marker.pose.orientation.w = 1.0 self.ground_truth_map_marker.scale.x = .025 self.ground_truth_map_marker.scale.y = .025 self.ground_truth_map_marker.scale.z = .025 self.ground_truth_map_marker.color.r = 0.0 self.ground_truth_map_marker.color.g = 1.0 self.ground_truth_map_marker.color.b = 0.0 self.ground_truth_map_marker.color.a = 1.0 self.ground_truth_map_marker.lifetime = rospy.Duration(1000) self.ground_truth_map_marker.points = sum( [[Point(p1[0], p1[1], 0), Point(p2[0], p2[1], 0)] for p1, p2 in ARENA], []) self.EKF_map_pub = rospy.Publisher("EKF_map", Marker, queue_size=10) self.EKF_map_marker = deepcopy(self.ground_truth_map_marker) self.EKF_map_marker.color.r = 1.0 self.EKF_map_marker.color.g = 0.0 self.EKF_map_marker.color.b = 0.0 self.EKF_map_marker.color.a = 1.0
def main(): # init node rospy.init_node("grasp_commander") # ========== publisher to jamming gripper ========== # grasp_pub = rospy.Publisher('/jamming_grasp', Int32, queue_size=1) # ========== Moveit init ========== # # moveit_commander init robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") arm_initial_pose = arm.get_current_pose().pose target_pose = geometry_msgs.msg.Pose() # Set the planning time arm.set_planning_time(10.0) # ========== TF Lister ========== # tf_buffer = tf2_ros.Buffer() tf_listner = tf2_ros.TransformListener(tf_buffer) for i in [0, 1]: target = "object_" + str(i) get_tf_flg = False # Get target TF while not get_tf_flg: try: trans = tf_buffer.lookup_transform('world', target, rospy.Time(0), rospy.Duration(10)) print "world -> ar_marker" print trans.transform print "Target Place & Pose" # Go to up from target target_pose.position.x = trans.transform.translation.x target_pose.position.y = trans.transform.translation.y target_pose.position.z = trans.transform.translation.z + 0.10 q = (trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w) (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(q) # roll -= math.pi/6.0 pitch += math.pi / 2.0 # yaw += math.pi/4.0 tar_q = tf.transformations.quaternion_from_euler( roll, pitch, yaw) target_pose.orientation.x = tar_q[0] target_pose.orientation.y = tar_q[1] target_pose.orientation.z = tar_q[2] target_pose.orientation.w = tar_q[3] print target_pose arm.set_pose_target(target_pose) arm.go() arm.clear_pose_targets() #rospy.sleep(2) # Get Grasp # waypoints = [] # waypoints.append(arm.get_current_pose().pose) # wpose = geometry_msgs.msg.Pose() # wpose.orientation.w = 1.0 # wpose.position.x = waypoints[0].position.x # wpose.position.y = waypoints[0].position.y - 0.085 # wpose.position.z = waypoints[0].position.z # waypoints.append(copy.deepcopy(wpose)) # (plan, fraction) = arm.compute_cartesian_path(waypoints, 0.005, 0.0, False) # target_pose.position.x = trans.transform.translation.x # target_pose.position.y = trans.transform.translation.y + 0.04 # target_pose.position.z = trans.transform.translation.z + 0.243 # target_pose.orientation = target_pose.orientation # print target_pose # arm.set_pose_target(target_pose) # arm.go() # arm.clear_pose_targets() # # Grasp # grasp_pub.publish(1) # print "!! Grasping !!" # rospy.sleep(2.0) # # Start Return # # waypoints = [] # # waypoints.append(arm.get_current_pose().pose) # # wpose = geometry_msgs.msg.Pose() # # wpose.orientation.w = 1.0 # # wpose.position.x = waypoints[0].position.x # # wpose.position.y = waypoints[0].position.y + 0.085 # # wpose.position.z = waypoints[0].position.z # # waypoints.append(copy.deepcopy(wpose)) # # (plan, fraction) = arm.compute_cartesian_path(waypoints, 0.005, 0.0, False) # target_pose.position.x = trans.transform.translation.x # target_pose.position.y = trans.transform.translation.y + 0.04 # target_pose.position.z = trans.transform.translation.z + 0.32 # target_pose.orientation = target_pose.orientation # print target_pose # arm.set_pose_target(target_pose) # arm.go() # arm.clear_pose_targets() # #rospy.sleep(2) # # Go to Home Position # target_pose.position.x = -0.13683 + (i-1)*0.08 # target_pose.position.y = -0.22166 # target_pose.position.z = 0.50554 # target_pose.orientation.x = 0.00021057 # target_pose.orientation.y = 0.70092 # target_pose.orientation.z = 0.00030867 # target_pose.orientation.w = 0.71324 # arm.set_pose_target(target_pose) # arm.go() # arm.clear_pose_targets() # #rospy.sleep(2) # target_pose.position.x = target_pose.position.x # target_pose.position.y = target_pose.position.y # target_pose.position.z -= 0.10 # target_pose.orientation = target_pose.orientation # arm.set_pose_target(target_pose) # arm.go() # arm.clear_pose_targets() # # Release # print " !! Release !!" # grasp_pub.publish(0) # rospy.sleep(1) get_tf_flg = True except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue
def __init__(self): self.marker_pose = Pose() self.tf_buffer = tf2_ros.Buffer( rospy.Duration(1200.0)) # tf buffer length self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
def __init__(self, robot_frame_id: str = "panda_link0", min_bound: Tuple[float, float, float] = (-1.0, -1.0, -1.0), max_bound: Tuple[float, float, float] = (1.0, 1.0, 1.0), normals_radius: float = 0.05, normals_max_nn: int = 30, include_color: bool = False, depth: int = 4, full_depth: int = 2, node_dis: bool = False, node_feature: bool = False, split_label: bool = False, adaptive: bool = False, adp_depth: int = 4, th_normal: float = 0.1, th_distance: float = 1.0, extrapolate: bool = False, save_pts: bool = False, key2xyz: bool = False, use_sim_time: bool = True, debug_draw: bool = False, debug_write_octree: bool = False, node_name: str = 'drl_grasping_octree_creator'): # Initialise node try: rclpy.init() except: if not rclpy.ok(): import sys sys.exit("ROS 2 could not be initialised") Node.__init__(self, node_name) self.set_parameters([ Parameter('use_sim_time', type_=Parameter.Type.BOOL, value=use_sim_time) ]) # Create tf2 buffer and listener for transform lookup self.__tf2_buffer = tf2_ros.Buffer() self.__tf2_listener = tf2_ros.TransformListener( buffer=self.__tf2_buffer, node=self) # Parameters self._robot_frame_id = robot_frame_id self._min_bound = min_bound self._max_bound = max_bound self._normals_radius = normals_radius self._normals_max_nn = normals_max_nn self._include_color = include_color self._depth = depth self._full_depth = full_depth self._debug_draw = debug_draw self._debug_write_octree = debug_write_octree # Create a converter between points and octree self._points_to_octree = ocnn.Points2Octree(depth=depth, full_depth=full_depth, node_dis=node_dis, node_feature=node_feature, split_label=split_label, adaptive=adaptive, adp_depth=adp_depth, th_normal=th_normal, th_distance=th_distance, extrapolate=extrapolate, save_pts=save_pts, key2xyz=key2xyz, bb_min=min_bound, bb_max=max_bound) # Spin executor in another thread self._executor = MultiThreadedExecutor(2) self._executor.add_node(self) self._executor.add_node(self.__tf2_listener.node) self._executor_thread = Thread(target=self._executor.spin, args=(), daemon=True) self._executor_thread.start()
#!/usr/bin/env python """ Node which transforms the cars pose data into the map frame and publishes it. """ import rospy import math import tf2_ros from geometry_msgs.msg import PoseStamped if __name__ == '__main__': rospy.init_node('transform_to_map') tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) pub_transform = rospy.Publisher('robot_pose', PoseStamped, queue_size=1) rate = rospy.Rate(10) while not rospy.is_shutdown(): try: trans = tfBuffer.lookup_transform('map', 'base_link', rospy.Time()) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rate.sleep() continue transform_msg = PoseStamped() transform_msg.header.frame_id = 'map' transform_msg.pose.position.x = trans.transform.translation.x
def __init__(self): rospy.init_node('marker_follower') # Set up a transform listener so we can lookup transforms in the past self.tfBuffer = tf2_ros.Buffer(rospy.Time(15)) self.lr = tf2_ros.TransformListener(self.tfBuffer) # Setup a transform broadcaster so that we can publish transforms # This allows to visualize the 3D position of the fiducial easily in rviz self.br = tf2_ros.TransformBroadcaster() # Velocity command topic cmd_vel_topic = rospy.get_param("~cmd_vel_topic", "/marker_follower/cmd_vel") # A publisher for robot motion commands self.cmdPub = rospy.Publisher(cmd_vel_topic, Twist, queue_size=1) # Flag to avoid sending repeated zero speeds self.suppressCmd = False # The name of the coordinate frame of the fiducial we are interested in self.target_fiducial = rospy.get_param("~target_fiducial", "marker_30") # Minimum distance we want the robot to be from the fiducial self.min_dist = rospy.get_param("~min_dist", 0.4) # Maximum distance a fiducial can be away for us to attempt to follow self.max_dist = rospy.get_param("~max_dist", 3.5) # Proportion of angular error to use as angular velocity self.angular_rate = rospy.get_param("~angular_rate", 2.0) # Maximum angular speed (radians/second) self.max_angular_rate = rospy.get_param("~max_angular_rate", 1.1) # Angular velocity when a fiducial is not in view self.lost_angular_rate = rospy.get_param("~lost_angular_rate", 0.4) # Proportion of linear error to use as linear velocity self.linear_rate = rospy.get_param("~linear_rate", 0.9) # Maximum linear speed (meters/second) self.max_linear_rate = rospy.get_param("~max_linear_rate", 0.15) # Linear speed decay (meters/second) self.linear_decay = rospy.get_param("~linear_decay", 0.9) # How many loop iterations to keep linear velocity after fiducial # disappears self.hysteresis_count = rospy.get_param("~hysteresis_count", 20) # How many loop iterations to keep rotating after fiducial disappears self.max_lost_count = rospy.get_param("~max_lost_count", 400) # Subscribe to incoming transforms rospy.Subscriber("/markers", MarkerList, self.newMarkers) self.fid_x = self.min_dist self.fid_y = 0 self.got_fid = False self.marker_spotted_init = False