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
