def setUp(self):
rospy.init_node('test_motion_execution_buffer')
self.tf = TransformListener()
self.obj_pub = rospy.Publisher('collision_object',
CollisionObject,
latch=True)
self.move_arm_action_client = actionlib.SimpleActionClient(
"move_right_arm", MoveArmAction)
self.move_arm_action_client.wait_for_server()
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1"
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.CYLINDER
obj1.shapes[0].dimensions = [float() for _ in range(2)]
obj1.shapes[0].dimensions[0] = .1
obj1.shapes[0].dimensions[1] = 1.5
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = .6
obj1.poses[0].position.y = -.6
obj1.poses[0].position.z = .75
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
self.obj_pub.publish(obj1)
rospy.sleep(2.0)
def __init__(self):
if not rospy.core.is_initialized():
rospy.init_node('laser_test')
rospy.loginfo("Initialised rospy node: laser_test")
self.tl = TransformListener()
self.lp = LaserProjection()
# Publishers
self.all_laser_pub = rospy.Publisher('/pepper/laser_2',
LaserScan,
queue_size=1)
self.pc_pub = rospy.Publisher('/cloud', PointCloud2, queue_size=1)
self.pcl_pub = rospy.Publisher('/cloudl', PointCloud2, queue_size=1)
self.pcr_pub = rospy.Publisher('/cloudr', PointCloud2, queue_size=1)
self.pc_redone_pub = rospy.Publisher('/cloud_redone',
PointCloud2,
queue_size=1)
self.pc_rereprojected_pub = rospy.Publisher('/cloud_rereprojected',
PointCloud2,
queue_size=1)
# Subscribers
left_sub = Subscriber('/pepper/scan_left', LaserScan)
front_sub = Subscriber('/pepper/scan_front', LaserScan)
right_sub = Subscriber('/pepper/scan_right', LaserScan)
self.ts = TimeSynchronizer([left_sub, front_sub, right_sub], 10)
rospy.loginfo("Finished intialising")
self.ddr = DDR('increment')
default_increment = radians(120.0 * 2.0) / 61.0
self.ddr.add_variable('angle_increment', '', default_increment, 0.05,
0.08)
# 130.665
self.ddr.add_variable('half_max_angle', '', 120., 115., 145.0)
self.ddr.start(self.dyn_rec_callback)
self.ts.registerCallback(self.scan_cb)
rospy.loginfo("Ready to go.")
def __init__(self):
self._camera_frame = rospy.get_param('%s/camera_frame' % (NODE_NAME, ),
'camera_rgb_optical_frame')
self._marker_frame = rospy.get_param('%s/marker_frame' % (NODE_NAME, ),
'marker26')
self._robot_base_frame = rospy.get_param(
'%s/robot_base_frame' % (NODE_NAME, ), 'world')
self._ee_frame = rospy.get_param('%s/ee_frame' % (NODE_NAME, ),
'left_arm_7_link')
self._save_calib_file = rospy.get_param(
'%s/save_calib_file' % (NODE_NAME, ), 'hand_eye_calib_data.npy')
self._load_calib_file = rospy.get_param(
'%s/load_calib_file' % (NODE_NAME, ), 'hand_eye_calib_data.npy')
self._calib_from_file = rospy.get_param(
'%s/calibrate_from_file' % (NODE_NAME, ), False)
rospy.loginfo("Camera frame: %s" % (self._camera_frame, ))
rospy.loginfo("Marker frame: %s" % (self._marker_frame, ))
rospy.loginfo("Robot frame: %s" % (self._robot_base_frame, ))
rospy.loginfo("End-effector frame: %s" % (self._ee_frame, ))
rospy.loginfo("Calibration filename to save: %s" %
(self._save_calib_file, ))
rospy.loginfo("Calibration filename to load: %s" %
(self._load_calib_file, ))
rospy.loginfo("Calibrate from file: %s" % (self._calib_from_file, ))
self._camera_base_p = [0.000, 0.022, 0.0]
self._camera_base_q = [-0.500, 0.500, -0.500, 0.500]
self._camera_base_transform = self.to_transform_matrix(
self._camera_base_p, self._camera_base_q)
self._calib = HandEyeCalib()
self._br = tf.TransformBroadcaster()
self._tf = TransformListener()
def __init__(self):
cv2.namedWindow("map")
cv2.namedWindow("past_map")
rospy.init_node("run_mapping")
#create map properties, helps to make ratio calcs
self.origin = [-10, -10]
self.seq = 0
self.resolution = 0.1
self.n = 200
self.pose = []
self.dyn_obs = []
self.rapid_appear = set()
self.counter = 0
#Giving initial hypotheses to the system
self.p_occ = 0.5 * np.ones(
(self.n, self.n)) #50-50 chance of being occupied
self.odds_ratio_hit = 3.0 #this is arbitrary, can re-assign
self.odds_ratio_miss = 0.3 #this is arbitrary, can reassign
#TODO: Evaluate these - what do we need to change in order to make this more friendly to our version? Potential changes:
#Whenever there is an adjustment to self.odds_ratio_miss, update an odds ratio that implies dynamicness
#calculates odds based upon hit to miss, equal odds to all grid squares
self.odds_ratios = (self.p_occ) / (1 - self.p_occ) * np.ones(
(self.n, self.n))
#calculate initial past odds_ratio
self.past_odds_ratios = (self.p_occ) / (1 - self.p_occ) * np.ones(
(self.n, self.n))
#TODO: Make sure that this is still an accurate representation of how probabilities work in our system. Make appropriate additions/adjustments for the dynamic obstacle piece
#write laser pubs and subs
rospy.Subscriber("scan", LaserScan, self.scan_received, queue_size=1)
self.pub = rospy.Publisher("map", OccupancyGrid)
#note - in case robot autonomy is added back in
self.tf_listener = TransformListener()
def __init__(self, server_prefix, group_name):
self.group_name = group_name
self.group = moveit_commander.MoveGroupCommander(self.group_name)
self._as = actionlib.SimpleActionServer(
server_prefix + self.group_name + "/manipulation",
ArmNavigationAction, self.action_cb)
self._as.start()
char = self.group_name[0]
self.jta = actionlib.SimpleActionClient(
'/' + char + '_arm_controller/joint_trajectory_action',
JointTrajectoryAction)
rospy.loginfo('Waiting for joint trajectory action')
self.jta.wait_for_server()
rospy.loginfo('Got it')
self.joint_state_sub = rospy.Subscriber("/joint_states",
JointState,
self.js_cb,
queue_size=1)
self.angles = None
self.move_to_pose_pub = rospy.Publisher(
"/pr2_arm_navigation/move_to_pose",
PoseStamped,
queue_size=10,
latch=True)
self.joint_names = [
char + '_shoulder_pan_joint', char + '_shoulder_lift_joint',
char + '_upper_arm_roll_joint', char + '_elbow_flex_joint',
char + '_forearm_roll_joint', char + '_wrist_flex_joint',
char + '_wrist_roll_joint'
]
self.tf_listener = TransformListener()
self.look_at_pub = rospy.Publisher("/art/robot/look_at",
PointStamped,
queue_size=1)
def __init__(self):
self.namespace = 'tello/'
self.hover_position = None
# subscribe to target pose
self.target_position = PoseStamped()
self.target_angles = [0.0, 0.0, 0.0]
# Initialize the tf listener
self.listener = TransformListener()
# initialize publisher for tello command velocity (geometry_Twist)
self.fly_msg = self.fly_tello_msg = Twist() # set the fly command
self.velocity_pub = rospy.Publisher(self.namespace + 'cmd_vel',
Twist,
queue_size=1)
self.takeoff_pub = rospy.Publisher(self.namespace + 'takeoff',
Empty,
queue_size=1)
self.land_pub = rospy.Publisher(self.namespace + 'land',
Empty,
queue_size=1)
def __init__(self, use_dummy_transform=False):
rospy.init_node('star_center_positioning_node')
if use_dummy_transform:
self.odom_frame_name = "odom_dummy"
else:
self.odom_frame_name = "odom"
self.marker_locators = {}
self.add_marker_locator(MarkerLocator(0,(-6*12*2.54/100.0,0),0))
self.add_marker_locator(MarkerLocator(1,(0.0,0.0),pi))
self.add_marker_locator(MarkerLocator(2,(0.0,10*12*2.54/100.0),0))
self.add_marker_locator(MarkerLocator(3,(-6*12*2.54/100.0,6*12*2.54/100.0),0))
self.pose_correction = rospy.get_param('~pose_correction',0.0)
self.marker_sub = rospy.Subscriber("ar_pose_marker",
ARMarkers,
self.process_markers)
self.odom_sub = rospy.Subscriber("odom", Odometry, self.process_odom, queue_size=10)
self.star_pose_pub = rospy.Publisher("STAR_pose",PoseStamped,queue_size=10)
self.continuous_pose = rospy.Publisher("STAR_pose_continuous",PoseStamped,queue_size=10)
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
def __init__(self):
if World.tf_listener == None:
World.tf_listener = TransformListener()
self._lock = threading.Lock()
self.surface = None
self._tf_broadcaster = TransformBroadcaster()
self._im_server = InteractiveMarkerServer('world_objects')
bb_service_name = 'find_cluster_bounding_box'
rospy.wait_for_service(bb_service_name)
self._bb_service = rospy.ServiceProxy(bb_service_name,
FindClusterBoundingBox)
rospy.Subscriber('interactive_object_recognition_result',
GraspableObjectList, self.receieve_object_info)
self._object_action_client = actionlib.SimpleActionClient(
'object_detection_user_command', UserCommandAction)
self._object_action_client.wait_for_server()
rospy.loginfo('Interactive object detection action ' +
'server has responded.')
self.clear_all_objects()
# The following is to get the table information
rospy.Subscriber('tabletop_segmentation_markers', Marker,
self.receieve_table_marker)
def __init__(self, context):
super(GaitGeneratorPlugin, self).__init__(context)
# Default values
self.gait_publisher = None
self.topic_name = ''
self.gait_directory = None
self.playback_speed = 100
self.time_slider_thread = None
self.current_time = 0
self.tf_listener = TransformListener()
self.joint_state_pub = rospy.Publisher('joint_states',
JointState,
queue_size=10)
self.joint_changed_history = RingBuffer(capacity=100, dtype=list)
self.joint_changed_redo_list = RingBuffer(capacity=100, dtype=list)
self.robot = urdf.Robot.from_parameter_server()
self.gait = ModifiableSubgait.empty_subgait(self, self.robot)
self.build_ui(context)
self.set_topic_name(self.topic_name_line_edit.text())
self.load_gait_into_ui()
def __init__(self):
rospy.init_node('thruster_controls')
self.sim = rospy.get_param('~/thruster_controls/sim')
if not self.sim:
self.pub = rospy.Publisher(self.ROBOT_PUB_TOPIC, ThrusterSpeeds, queue_size=3)
else:
self.pub = rospy.Publisher(self.SIM_PUB_TOPIC, Float32MultiArray, queue_size=3)
self.enable_service = rospy.Service('enable_controls', SetBool, self.handle_enable_controls)
self.tm = ThrusterManager(rr.get_filename('package://controls/config/cthulhu.config', use_protocol=False))
self.listener = TransformListener()
for d in utils.get_axes():
rospy.Subscriber(utils.get_controls_move_topic(d), Float64, self._on_pid_received, d)
rospy.Subscriber(utils.get_power_topic(d), Float64, self._on_power_received, d)
self.pid_outputs = np.zeros(6)
self.pid_outputs_local = np.zeros(6)
self.powers = np.zeros(6)
self.t_allocs = np.zeros(8)
def test_tf(self):
rospy.init_node('morse_ros_tf_test')
self.tf = TransformListener()
sleep(1)
self.assertTrue(self.tf.frameExists("/odom"))
self.assertTrue(self.tf.frameExists("/base_footprint"))
self.assertTrue(self.tf.frameExists("/map"))
self.assertTrue(self.tf.frameExists("/robot2"))
self._check_pose("odom", "base_footprint", [0, 0, 0], [0, 0, 0, 1])
self._check_pose("map", "robot2", [0, 0, 0], [0, 0, 0, 1])
with Morse() as morse:
teleport = morse.robot.motion
teleport.publish({'x' : 2, 'y' : 0, 'z' : 0, \
'yaw' : 0, 'pitch' : 0.0, 'roll' : 0.0})
morse.sleep(0.1)
self._check_pose("odom", "base_footprint", [2, 0, -0.1], [0, 0, 0, 1])
def ros_setup(self):
"""Creates and inits ROS components"""
RComponent.ros_setup(self)
self.tf = TransformListener()
self.move_action_client = actionlib.SimpleActionClient(
self.move_action_name, MoveAction)
self.dock_action_client = actionlib.SimpleActionClient(
self.dock_action_name, DockAction)
self.start_docking_server = rospy.Service(
'~start', Trigger, self.start_docking_service_cb)
self.stop_docking_server = rospy.Service('~stop', Trigger,
self.stop_docking_service_cb)
self.save_results_server = rospy.Service('~save_results', Empty,
self.save_results_service_cb)
self.docking_status = rospy.Publisher('~status', String, queue_size=10)
return 0
def __init__(self):
self.node_name = rospy.get_name()
self.tf_listener = TransformListener()
# Re-publishing Robot's states in the format needed for the network
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
self.pose_pub = rospy.Publisher('/robot_pose',
PoseStamped,
queue_size=10)
self.vel_pub = rospy.Publisher('/robot_velocity',
Vector3,
queue_size=10)
# Re-publishing Pedestrians states in the format needed for the network
self.ped_sub = rospy.Subscriber(
'/spencer/perception_internal/detected_person_association/lasers_upper_body_fused',
DetectedPersons, self.peds_callback)
self.clusters_pub = rospy.Publisher('cluster/output/clusters',
Clusters,
queue_size=1)
#self.pub_agent_markers = rospy.Publisher('~agent_markers',MarkerArray,queue_size=1)
#publish goal
self.goal_pub = rospy.Publisher('move_base_simple/goal',
PoseStamped,
queue_size=1)
def __init__(self, arm_parameters):
"""
Initializes the Palbator Moveit arm controller.
"""
moveit_commander.roscpp_initialize(sys.argv)
# rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
self._parameters = arm_parameters
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander(
self._parameters['Palbator_arm_move_group'])
self.display_trajectory_publisher = rospy.Publisher(
self._parameters['display_arm_planned_path_topic'],
moveit_msgs.msg.DisplayTrajectory,
queue_size=10)
self._tflistener = TransformListener()
self.arm_length = self._parameters['Palbator_arm_length']
rospy.logwarn("{class_name} : ARM CONTROLLER ON".format(
class_name=self.__class__.__name__))
def __init__(self):
# Initialize arm state.
self._tf_listener = TransformListener()
self._arm = Arm(self._tf_listener)
self._arm.close_gripper()
self._status = ExecutionStatus.NOT_EXECUTING
rospy.Service('/fetch_pbd/move_arm_to_joints_plan', MoveArm,
self._move_to_joints_plan)
rospy.Service('/fetch_pbd/move_arm_to_joints', MoveArmTraj,
self._move_to_joints)
rospy.Service('/fetch_pbd/move_arm_to_pose', MoveArm,
self._move_to_pose)
rospy.Service('/fetch_pbd/start_move_arm_to_pose', MoveArm,
self._start_move_to_pose)
rospy.Service('/fetch_pbd/is_reachable', MoveArm, self._is_reachable)
rospy.Service('/fetch_pbd/is_arm_moving', GetArmMovement,
self._is_arm_moving)
rospy.Service('/fetch_pbd/relax_arm', Empty, self._relax_arm)
rospy.Service('/fetch_pbd/reset_arm_movement_history', Empty,
self._reset_movement_history)
rospy.Service('/fetch_pbd/get_gripper_state', GetGripperState,
self._get_gripper_state)
rospy.Service('/fetch_pbd/get_ee_pose', GetEEPose, self._get_ee_pose)
rospy.Service('/fetch_pbd/get_joint_states', GetJointStates,
self._get_joint_states)
rospy.Service('/fetch_pbd/set_gripper_state', SetGripperState,
self._set_gripper_state)
rospy.loginfo('Arm initialized.')
def __init__(self):
print('Initializing')
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('localizer')
self.pf = ParticleFilter()
self.base_frame = "base_link" # Robot base frame
self.map_frame = "map" # Map coord frame
self.odom_frame = "odom" # Odom coord frame
self.scan_topic = "scan" # Laser scan topic
self.linear_threshold = 0.1 # the amount of linear movement before performing an update
self.angular_threshold = math.pi / 10 # the amount of angular movement before performing an update
self.max_dist = 2.0 # maximum penalty to assess in the likelihood field model
self.odom_pose = PoseStamped()
self.robot_pose = Pose()
self.robot_pose = Pose()
self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.process_scan)
# init pf
# subscribers and publisher
self.odom_sub = rospy.Subscriber("/odom", Odometry,
self.odom_particle_updater)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.pose_updater)
# enable listening for and broadcasting coord transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.current_odom_xy_theta = []
print("initialization complete")
self.initialized = True
def __init__(self, name, odom_frame, base_frame):
"""
@param name: the name of the action
@param odom_frame: the frame the robot is moving in (odom_combined)
@param base_frame: the vehicles own frame (usually base_link)
"""
self._action_name = name
self.__odom_frame = odom_frame
self.__base_frame = base_frame
self.__server = actionlib.SimpleActionServer(self._action_name,
sprayAction,
auto_start=False)
self.__server.register_preempt_callback(self.preempt_cb)
self.__server.register_goal_callback(self.goal_cb)
self.__cur_pos = 0
self.__start_pos = 0
self.__start_yaw = 0
self.__cur_yaw = 0
self.__feedback = sprayFeedback()
self.__listen = TransformListener()
#self.vel_pub = rospy.Publisher("/fmControllers/cmd_vel_auto",Twist)
self.spray_pub = rospy.Publisher("/fmControllers/spray", UInt32)
self.__start_time = rospy.Time.now()
self.new_goal = False
self.n_sprays = 0
self.spray_msg = UInt32()
self.spray_l = False
self.spray_cnt = 0
self.__server.start()
def __init__(self,
moveit_group,
initial_joint_pos=INITIAL_JOINT_STATE,
control_mode='position',
extended_finger=True):
"""
Sawyer class.
:param initial_joint_pos: {str: float}
{'joint_name': position_value}, and also
initial_joint_pos should include all of the
joints that user wants to control and observe.
:param moveit_group: str
Use this to check safety
:param control_mode: string
robot control mode: 'position' or velocity
or effort
"""
Robot.__init__(self)
self._limb = intera_interface.Limb('right')
self._gripper = intera_interface.Gripper()
self._initial_joint_pos = initial_joint_pos
self._control_mode = control_mode
self._used_joints = []
for joint in initial_joint_pos:
self._used_joints.append(joint)
self._joint_limits = rospy.wait_for_message('/robot/joint_limits',
JointLimits)
self._moveit_group = moveit_group
self._tf_listener = TransformListener()
self._base_frame = "base"
if extended_finger:
self._tip_frame = "right_gripper_tip_ex"
else:
self._tip_frame = "right_gripper_tip"
self._sv = StateValidity()
def __init__(self):
rospy.loginfo("Waiting for the tf to come up")
rospy.sleep(2)
rospy.init_node('sweep_nav')
sub = rospy.Subscriber('/imu/mag',
MagneticField,
self.callback,
queue_size=10)
self.pub = rospy.Publisher('/mobile_base/commands/velocity',
Twist,
queue_size=10)
self.pub2 = rospy.Publisher('way_point', Marker, queue_size=10)
self.pub3 = rospy.Publisher('z_mag', Marker, queue_size=10)
self.marker = Marker()
self.id_new = 0
self.id_old = 0
self.count = 1
self.z = []
self.marker.header.frame_id = 'map'
self.marker.type = 3
self.marker.action = self.marker.ADD
self.marker.scale.x = 0.1
self.marker.scale.y = 0.1
self.marker.color.a = 0.5
self.marker.scale.z = 0.1
self.tf = TransformListener()
self.vel = Twist()
self.speed = 0.15
self.X_KOUSHI = 150
self.Y_KOUSHI = 150
self.id_list = [[i + self.X_KOUSHI * j for i in range(self.X_KOUSHI)]
for j in range(self.Y_KOUSHI)]
self.mag = 0
self.f = open('../map/way.txt', 'a')
self.now = rospy.get_time()
def __init__(self, configs):
"""
Constructor of the SimpleCamera class.
:param configs: Camera specific configuration object
:type configs: YACS CfgNode
"""
super(SimpleCamera, self).__init__(configs=configs)
self.cv_bridge = CvBridge()
self.camera_info_lock = threading.RLock()
self.camera_img_lock = threading.RLock()
self._tf_listener = TransformListener()
self.rgb_img = None
self.depth_img = None
self.camera_info = None
self.camera_P = None
rospy.Subscriber(self.configs.CAMERA.ROSTOPIC_CAMERA_INFO_STREAM,
CameraInfo, self._camera_info_callback)
rgb_topic = self.configs.CAMERA.ROSTOPIC_CAMERA_RGB_STREAM
self.rgb_sub = message_filters.Subscriber(rgb_topic, Image)
depth_topic = self.configs.CAMERA.ROSTOPIC_CAMERA_DEPTH_STREAM
self.depth_sub = message_filters.Subscriber(depth_topic, Image)
img_subs = [self.rgb_sub, self.depth_sub]
self.sync = message_filters.ApproximateTimeSynchronizer(img_subs,
queue_size=10,
slop=0.2)
self.sync.registerCallback(self._sync_callback)
depth_threshold = (self.configs.BASE.VSLAM.DEPTH_MIN,
self.configs.BASE.VSLAM.DEPTH_MAX)
cfg_filename = self.configs.BASE.VSLAM.CFG_FILENAME
self.depth_cam = DepthImgProcessor(subsample_pixs=1,
depth_threshold=depth_threshold,
cfg_filename=cfg_filename)
self.cam_cf = self.configs.BASE.VSLAM.RGB_CAMERA_CENTER_FRAME
self.base_f = self.configs.BASE.VSLAM.VSLAM_BASE_FRAME
def __init__(self):
self.choice = 0
self.movePub = rospy.Publisher(
'/cmd_vel', Twist, queue_size=10) # publisher for move commands
self.peopleSub = rospy.Subscriber(
'/people_tracker_filter/people', People,
self.peopleCall) # subscribe to people tracker
self.laserSub = rospy.Subscriber('/scan', LaserScan,
self.laserCall) # subscribe to laser
self.distance = 0 # variable to store distance from laser
self.tf = TransformListener()
while True:
print("select behaviour")
print("1) control")
print("2) friendly")
print("3) cautious")
self.choice = input("> ")
if self.choice == 1 or self.choice == 2:
break
elif self.choice == 3:
break
print("started")
def read_from_pickle():
global points_description, tf_listner
rospy.init_node('point_test')
tf_listner = TransformListener()
points_description = pickle.load(
open(
rospkg.RosPack().get_path('robot_describe') +
"/script/data/points_describtion.p", "rb"))
position_points = [[i[0].x, i[0].y] for i in points_description]
description_degree = [[i[1], i[2]] for i in points_description]
# for points in description_degree:
# print points
points_description = [position_points, description_degree]
print description_degree
robot = Robot(True,
2,
start_index_dataset=1140,
use_direction_file=False,
bag_name="/bag_train/")
rospy.Subscriber("/robot_0/base_pose_ground_truth", Odometry,
callback_robot_0, robot)
rospy.Subscriber("/robot_0/base_scan_1", LaserScan, callback_laser_scan,
robot)
rospy.spin()
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('RMI_pf')
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 20
self.linear_mov = 0.1
self.angular_mov = math.pi / 10
self.laser_max_distance = 2.0
self.sigma = 0.05
# Descomentar essa linha caso /initialpose seja publicada
# self.pose_listener = rospy.Subscriber("initialpose",
# PoseWithCovarianceStamped,
# self.update_initial_pose)
self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan,
self.scan_received)
self.particle_pub = rospy.Publisher("particlecloud_rmi",
PoseArray,
queue_size=1)
self.tf_listener = TransformListener()
self.particle_cloud = []
self.current_odom_xy_theta = []
self.map_server = rospy.ServiceProxy('static_map', GetMap)
self.map = self.map_server().map
self.occupancy_field = OccupancyField(self.map)
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame,
rospy.Time(), rospy.Duration(1.0))
self.initialized = True
def __init__(self):
self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.listener = TransformListener()
rospy.Subscriber("joy", Joy, self._joyChanged)
rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged)
self.cmd_vel_telop = Twist()
#self.pidX = PID(20, 12, 0.0, -30, 30, "x")
#self.pidY = PID(-20, -12, 0.0, -30, 30, "y")
#self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z")
#self.pidYaw = PID(50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
self.pidX = PID(20, 12.0, 0.2, -30, 30, "x")
self.pidY = PID(-20, -12.0, -0.2, -30, 30, "y")
#50000 800
self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 57000, "z")
self.pidYaw = PID(50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
self.state = Controller.Manual
self.targetZ = 1
self.targetX = 0.0
self.targetY = -1.0
self.des_angle = 90.0
self.lastZ = 0.0
self.power = 50000.0
self.pubVz = rospy.Publisher('vel_z', Float32, queue_size=1)
self.lastJoy = Joy()
def __init__(self):
rospy.init_node('thruster_controls')
self.sim = rospy.get_param('~/thruster_controls/sim')
if self.sim == False:
self.pub = rospy.Publisher(self.ROBOT_PUB_TOPIC, ThrusterSpeeds, queue_size=3)
elif self.sim == True:
self.pub = rospy.Publisher(self.SIM_PUB_TOPIC, Float32MultiArray, queue_size=3)
else:
# TODO: alert that unrecognized mode destination has been set
pass
self.enable_service = rospy.Service('enable_controls', SetBool, self.handle_enable_controls)
self.tm = ThrusterManager(os.path.join(sys.path[0], '../config/cthulhu.config'))
self.listener = TransformListener()
rospy.Subscriber(self.CONTROLS_MOVE_X_TOPIC, Float64, self._on_x)
rospy.Subscriber(self.CONTROLS_MOVE_Y_TOPIC, Float64, self._on_y)
rospy.Subscriber(self.CONTROLS_MOVE_Z_TOPIC, Float64, self._on_z)
rospy.Subscriber(self.CONTROLS_MOVE_ROLL_TOPIC, Float64, self._on_roll)
rospy.Subscriber(self.CONTROLS_MOVE_PITCH_TOPIC, Float64, self._on_pitch)
rospy.Subscriber(self.CONTROLS_MOVE_YAW_TOPIC, Float64, self._on_yaw)
rospy.Subscriber(self.CONTROLS_POWER_X_TOPIC, Float64, self._on_x_power)
rospy.Subscriber(self.CONTROLS_POWER_Y_TOPIC, Float64, self._on_y_power)
rospy.Subscriber(self.CONTROLS_POWER_Z_TOPIC, Float64, self._on_z_power)
rospy.Subscriber(self.CONTROLS_POWER_ROLL_TOPIC, Float64, self._on_roll_power)
rospy.Subscriber(self.CONTROLS_POWER_PITCH_TOPIC, Float64, self._on_pitch_power)
rospy.Subscriber(self.CONTROLS_POWER_YAW_TOPIC, Float64, self._on_yaw_power)
self.pid_outputs = np.zeros(6)
self.pid_outputs_local = np.zeros(6)
self.powers = np.zeros(6)
self.t_allocs = np.zeros(8)
def __init__(self):
cv2.namedWindow("map")
cv2.namedWindow("past_map")
rospy.init_node("run_mapping")
#create map properties, helps to make ratio calcs
self.origin = [-10, -10]
self.seq = 0
self.resolution = 0.1
self.n = 200
self.pose = []
self.cluster_pose = []
self.dyn_obs = []
self.rapid_appear = set()
self.counter = 0
#Giving initial hypotheses to the system
self.p_occ = 0.5 * np.ones(
(self.n, self.n)) #50-50 chance of being occupied
self.odds_ratio_hit = 3.0 #this is arbitrary, can re-assign
self.odds_ratio_miss = 0.3 #this is arbitrary, can reassign
#calculates odds based upon hit to miss, equal odds to all grid squares
self.odds_ratios = (self.p_occ) / (1 - self.p_occ) * np.ones(
(self.n, self.n))
#calculate initial past odds_ratio
self.past_odds_ratios = (self.p_occ) / (1 - self.p_occ) * np.ones(
(self.n, self.n))
#write laser pubs and subs
rospy.Subscriber("scan", LaserScan, self.scan_received, queue_size=1)
self.pub = rospy.Publisher("map", OccupancyGrid)
#note - in case robot autonomy is added back in
self.tf_listener = TransformListener()
def __init__(self, topic='/hace/people', x_min=0, x_max=0.5, y_min=-0.75, y_max=0.05, z_min=-0.25, z_max=0.55):
super(HandoverAdaptionInit, self).__init__(outcomes=['right_hand_in_ws', 'left_hand_in_ws', 'error'])
rospy.loginfo('Starting initiation state.')
# define workspace
self._x_min = x_min
self._x_max = x_max
self._y_min = y_min
self._y_max = y_max
self._z_min = z_min
self._z_max = z_max
self._ws_transformed = False
self._tf = TransformListener()
self._topic = topic
self._person = None
self._persons = None
self._sub = ProxySubscriberCached({self._topic: MinimalHumans})
self._error = False
def __init__(self):
self.robot = hsrb_interface.Robot()
self.noise = 0.1
self.omni_base = self.robot.get('omni_base')
self.whole_body = self.robot.get('whole_body')
self.gripper = self.robot.get('gripper')
self.tl = TransformListener()
self.start_recording = False
self.stop_recording = False
self.com = COM()
self.com.go_to_initial_state(self.whole_body, self.gripper)
#self.whole_body.move_to_joint_positions({'head_tilt_joint':-0.3})
self.cam = RGBD()
self.joystick = JoyStick()
self.torque = Gripper_Torque()
self.joints = Joint_Positions()
def __init__(self):
# Get information for this particular camera
camera_info = get_single('head_camera/depth_registered/camera_info',
CameraInfo)
print camera_info
# Set information for the camera
self.cam_model = PinholeCameraModel()
self.cam_model.fromCameraInfo(camera_info)
# Subscribe to the camera's image topic and depth image topic
self.rgb_image_sub = rospy.Subscriber("head_camera/rgb/image_raw",
Image, self.on_rgb)
self.depth_image_sub = rospy.Subscriber(
"head_camera/depth_registered/image_raw", Image, self.on_depth)
# Publish where the button is in the base link frame
self.point_pub = rospy.Publisher('button_point',
PointStamped,
queue_size=10)
# Set up connection to CvBridge
self.bridge = CvBridge()
# Open up viewing windows
cv2.namedWindow('depth')
cv2.namedWindow('rgb')
# Set up the class variables
self.rgb_image = None
self.rgb_image_time = None
self.depth_image = None
self.center = None
self.return_point = PointStamped()
self.tf_listener = TransformListener()
def pid_handle():
""" Temporary function to cope with the keyboard input error of click
"""
global car_rotation, car_translation
scan_listener = TransformListener()
rospy.init_node("pid_local", anonymous=True)
MASTER_MAP.intial_state_listener("/map", "/odom")
way_point_coord = []
for i in WAY_POINT_GRID:
way_point_coord.append(MASTER_MAP.grid_to_coord(i))
# get initial time to setup the pid controller
init_time = rospy.Time.now().secs + rospy.Time.now().nsecs * (10**-9)
# set pit controller
pid_driver = PIDController(MAX_SPEED, MAX_ANGLE, PID_CONST, init_time,
way_point_coord)
point_export = rospy.Publisher(PATH_TOPIC, Float32MultiArray, queue_size=2)
rospy.Subscriber(
ODOM_TOPIC,
Odometry,
callback_pid,
callback_args=[pid_driver, pid_pub, point_export],
)
rospy.Subscriber(ODOM_TOPIC, Odometry, odom_tf_callback, queue_size=10)
rospy.Subscriber(LASER_TOPIC, LaserScan, scan_callback, queue_size=10)
