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",
"rotate_HOME"
]
# 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
# - Status for Web
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"
# - Others
self.fsm_state = "STANDBY"
self.restore = False
# 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.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)
self.restore_srv = rospy.Service("/restore/call", Empty,
self.restoreSRV)
self.cancel_srv = rospy.Service("/mission/cancel", Empty,
self.cancelSRV)
# 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_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)
self.path_cancel_call = rospy.ServiceProxy("/path/cancel", Empty)
# Startup
self.sound_call(self.default_sound)
# Main Loop()
self.main_loop()
def run(self):
try:
vision = VisionProxy()
tf_buffer = tf2_ros.Buffer(
rospy.Duration(1200.0)) #tf buffer length
tf_listener = tf2_ros.TransformListener(tf_buffer)
i = raw_input("Enter the number of the part to be picked up:")
i = int(i)
if i > 0:
rospy.logdebug("find object")
expected_position = geometry_msgs.msg.PoseStamped()
object_id = str(
i
) # The part number (see the definition in o2as_parts_description)
item_pose = vision.find_object(expected_position,
position_tolerance=0.2,
object_id=object_id,
camera="b_bot_camera")
if item_pose != None:
# Transform pose from camera to workspace frame, so we can set the orientation more easily
target_frame = "workspace_center"
source_frame = item_pose.header.frame_id
transform = tf_buffer.lookup_transform(
target_frame, source_frame, rospy.Time(0),
rospy.Duration(1.0))
pick_pose = tf2_geometry_msgs.do_transform_pose(
item_pose, transform)
downward_orientation = geometry_msgs.msg.Quaternion(
*tf_conversions.transformations.quaternion_from_euler(
0, pi / 2, 0))
pick_pose.pose.orientation = downward_orientation
rospy.logdebug("pick object")
self.publish_marker(pick_pose, "pick_pose") # debug
self.pick("b_bot",
pick_pose,
grasp_height=0.1,
approach_height=0.1,
speed_fast=0.2,
speed_slow=0.02,
gripper_command="easy_pick_only_inner")
# place_pose = pick_pose
# rospy.logdebug("place object")
# self.place("b_bot", place_pose, place_height = 0.03, approach_height = 0.05,
# speed_fast = 0.2, speed_slow = 0.02, gripper_command="easy_pick_only_inner",
# lift_up_after_place = False)
else:
rospy.loginfo("No valid entry. Skipping.")
# rospy.logdebug("Go to home position")
# self.go_to_named_pose("home_c", "c_bot")
# self.go_to_named_pose("home_b", "b_bot")
# self.go_to_named_pose("home_a", "a_bot")
rospy.loginfo("============ Done!")
except rospy.ROSInterruptException:
pass
except KeyError:
logger.logwarn('No parameters found on the parameter server')
exit(0)
expected_keys = ['parent', 'child', 'rotation', 'translation']
params_list = []
for key, data in params.items():
if cu.misc.has_keys(data, expected_keys):
params_list.append(data)
if len(params_list) == 0:
logger.logwarn('No transformations found on the parameter server')
exit(0)
# Publish tf data
num_tfs = len(params_list)
rospy.loginfo('Publishing {0} transformation(s) to /tf'.format(num_tfs))
tf_broadcaster = tf2_ros.TransformBroadcaster()
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
rate = rospy.Rate(publish_rate)
tf_msg = TransformStamped()
while not rospy.is_shutdown():
for params in params_list:
trans = np.array(params['translation'])
rot = np.array(params['rotation'])
child = params['child']
parent = params['parent']
tf_msg.header.stamp = rospy.Time.now()
if invert:
T = br.quaternion.to_transform(rot)
T[:3, 3] = trans
Tinv = br.transform.inverse(T)
tf_msg.transform = cu.conversions.to_transform(Tinv)
#!/usr/bin/env python3
import rospy
import tf2_ros
from gazebo_msgs.msg import ModelStates
from geometry_msgs.msg import TransformStamped
import transforms3d
import numpy as np
rospy.init_node("localization_faker")
tf_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(10.0))
tf_listener = tf2_ros.TransformListener(tf_buffer)
br = tf2_ros.TransformBroadcaster()
def model_state_to_tf(model_state_msg):
t = TransformStamped()
t.header.stamp = rospy.Time.now()
for i, name in enumerate(model_state_msg.name):
t.header.frame_id = name + "/map"
t.child_frame_id = name + "/odom"
try:
robot_in_odom = tf_buffer.lookup_transform(
name + "/odom",
name + "/base_link",
t.header.stamp,
timeout=rospy.Duration(0.1))
except tf2_ros.LookupException as ex:
rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex))
return
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'])
while laser_range[0] > stop_distance:
movebot()
stopbot()
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('Distancessssssss: %s', str(lri))
rospy.loginfo(['move!'])
move()
else:
lri[0] = []
# if the list is not empty
if (len(lri[0]) > 0):
rospy.loginfo(['Stop!'])
# find direction with the largest distance from the Lidar
# rotate to that direction
# start moving
rospy.loginfo(['running our function'])
move()
# 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()
def __init__(self, inertial_frame_id='world'):
"""Class constructor."""
#assert inertial_frame_id in ['world', 'world_ned']
# Reading current namespace
self._namespace = rospy.get_namespace()
self._logger = logging.getLogger('vehicle_model')
self._inertial_frame_id = inertial_frame_id
self._body_frame_id = None
if self._inertial_frame_id == 'world_ned':
self._body_frame_id = 'anahita/base_link'
else:
self._body_frame_id = 'base_link_ned'
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)
tf_trans_ned_to_enu = None
try:
tf_trans_ned_to_enu = tf_buffer.lookup_transform(
'world', 'world_ned', rospy.Time(), rospy.Duration(1))
except Exception as e:
self._logger.error('No transform found between world and the '
'world_ned ' + self.namespace)
self._logger.error(str(e))
self.transform_ned_to_enu = None
if tf_trans_ned_to_enu is not None:
self.transform_ned_to_enu = quaternion_matrix(
(tf_trans_ned_to_enu.transform.rotation.x,
tf_trans_ned_to_enu.transform.rotation.y,
tf_trans_ned_to_enu.transform.rotation.z,
tf_trans_ned_to_enu.transform.rotation.w))[0:3, 0:3]
print('Transform world_ned (NED) to world (ENU)=\n' +
str(self.transform_ned_to_enu))
self._mass = 0
if rospy.has_param('~mass'):
self._mass = rospy.get_param('~mass')
if self._mass <= 0:
raise rospy.ROSException('Mass has to be positive')
else:
self._mass = 862.87
self._inertial = dict(ixx=0, iyy=0, izz=0, ixy=0, ixz=0, iyz=0)
if rospy.has_param('~inertial'):
print('has inertial')
inertial = rospy.get_param('~inertial')
for key in self._inertial:
if key not in inertial:
raise rospy.ROSException('Invalid moments of inertia')
self._inertial = inertial
else:
self._inertial['ixx'] = 243.39
self._inertial['iyy'] = 367.20
self._inertial['izz'] = 319.23
self._inertial['ixy'] = 1.44
self._inertial['ixz'] = 33.41
self._inertial['iyz'] = 2.6
self._cog = [0, 0, 0]
if rospy.has_param('~cog'):
self._cog = rospy.get_param('~cog')
if len(self._cog) != 3:
raise rospy.ROSException('Invalid center of gravity vector')
self._cob = [0, 0, 0]
if rospy.has_param('~cob'):
self._cob = rospy.get_param('~cob')
if len(self._cob) != 3:
raise rospy.ROSException('Invalid center of buoyancy vector')
else:
self._cob = [0, 0, 0.05]
self._body_frame = 'base_link'
if rospy.has_param('~base_link'):
self._body_frame = rospy.get_param('~base_link')
self._volume = 0.0
if rospy.has_param('~volume'):
self._volume = rospy.get_param('~volume')
if self._volume <= 0:
raise rospy.ROSException('Invalid volume')
else:
self._volume = 0.5
# Fluid density
self._density = 1028.0
if rospy.has_param('~density'):
self._density = rospy.get_param('~density')
if self._density <= 0:
raise rospy.ROSException('Invalid fluid density')
# Bounding box
self._height = 0.4
self._length = 1.0
self._width = 0.5
if rospy.has_param('~height'):
self._height = rospy.get_param('~height')
if self._height <= 0:
raise rospy.ROSException('Invalid height')
if rospy.has_param('~length'):
self._length = rospy.get_param('~length')
if self._length <= 0:
raise rospy.ROSException('Invalid length')
if rospy.has_param('~width'):
self._width = rospy.get_param('~width')
if self._width <= 0:
raise rospy.ROSException('Invalid width')
# Calculating the rigid-body mass matrix
self._M = np.zeros(shape=(6, 6), dtype=float)
self._M[0:3, 0:3] = self._mass * np.eye(3)
self._M[0:3, 3:6] = - self._mass * \
cross_product_operator(self._cog)
self._M[3:6, 0:3] = self._mass * \
cross_product_operator(self._cog)
self._M[3:6, 3:6] = self._calc_inertial_tensor()
# Loading the added-mass matrix
self._Ma = np.zeros((6, 6))
if rospy.has_param('~Ma'):
self._Ma = np.array(rospy.get_param('~Ma'))
if self._Ma.shape != (6, 6):
raise rospy.ROSException('Invalid added mass matrix')
else:
self._Ma[0][0] = 379.79
self._Ma[0][1] = -3.8773
self._Ma[0][2] = -53.32
self._Ma[0][3] = 4.5426
self._Ma[0][4] = -85.54
self._Ma[0][5] = -3.8033
self._Ma[1][0] = -3.8773
self._Ma[1][1] = 622
self._Ma[1][2] = 25.29
self._Ma[1][3] = 209.44
self._Ma[1][4] = -2.8488
self._Ma[1][5] = 32.726
self._Ma[2][0] = -53.32
self._Ma[2][1] = 25.29
self._Ma[2][2] = 1959.9
self._Ma[2][3] = 3.1112
self._Ma[2][4] = -196.42
self._Ma[2][5] = 5.774
self._Ma[3][0] = 4.5426
self._Ma[3][1] = 209.44
self._Ma[3][2] = 3.1112
self._Ma[3][3] = 264.9
self._Ma[3][4] = -5.027
self._Ma[3][5] = 11.019
self._Ma[4][0] = -85.54
self._Ma[4][1] = -2.8488
self._Ma[4][2] = -196.42
self._Ma[4][3] = -5.027
self._Ma[4][4] = 442.69
self._Ma[4][5] = -1.1162
self._Ma[5][0] = -3.8033
self._Ma[5][1] = 32.726
self._Ma[5][2] = 5.775
self._Ma[5][3] = 11.019
self._Ma[5][4] = -1.1162
self._Ma[5][5] = 114.32
# Sum rigid-body and added-mass matrices
self._Mtotal = np.zeros(shape=(6, 6))
self._calc_mass_matrix()
# Acceleration of gravity
self._gravity = 9.81
# Initialize the Coriolis and centripetal matrix
self._C = np.zeros((6, 6))
# Vector of restoring forces
self._g = np.zeros(6)
# Loading the linear damping coefficients
self._linear_damping = np.zeros(shape=(6, 6))
if rospy.has_param('~linear_damping'):
self._linear_damping = np.array(rospy.get_param('~linear_damping'))
if self._linear_damping.shape == (6, ):
self._linear_damping = np.diag(self._linear_damping)
if self._linear_damping.shape != (6, 6):
raise rospy.ROSException(
'Linear damping must be given as a 6x6 matrix or the diagonal coefficients'
)
else:
self._linear_damping[0][0] = -34.82
self._linear_damping[1][1] = -39.48
self._linear_damping[2][2] = -368.4
self._linear_damping[3][3] = -138.8
self._linear_damping[4][4] = -159.77
self._linear_damping[5][5] = -55
# Loading the nonlinear damping coefficients
self._quad_damping = np.zeros(shape=(6, ))
if rospy.has_param('~quad_damping'):
self._quad_damping = np.array(rospy.get_param('~quad_damping'))
if self._quad_damping.shape != (6, ):
raise rospy.ROSException(
'Quadratic damping must be given defined with 6 coefficients'
)
else:
self._quad_damping[0] = -378.22
self._quad_damping[1] = -502.53
self._quad_damping[2] = -921.01
self._quad_damping[3] = -342
self._quad_damping[4] = -394.44
self._quad_damping[5] = -263.27
# Loading the linear damping coefficients proportional to the forward speed
self._linear_damping_forward_speed = np.zeros(shape=(6, 6))
if rospy.has_param('~linear_damping_forward_speed'):
self._linear_damping_forward_speed = np.array(
rospy.get_param('~linear_damping_forward_speed'))
if self._linear_damping_forward_speed.shape == (6, ):
self._linear_damping_forward_speed = np.diag(
self._linear_damping_forward_speed)
if self._linear_damping_forward_speed.shape != (6, 6):
raise rospy.ROSException(
'Linear damping proportional to the forward speed must be given as a 6x6 '
'matrix or the diagonal coefficients')
# Initialize damping matrix
self._D = np.zeros((6, 6))
# Vehicle states
self._pose = dict(pos=np.zeros(3), rot=quaternion_from_euler(0, 0, 0))
# Velocity in the body frame
self._vel = np.zeros(6)
# Acceleration in the body frame
self._acc = np.zeros(6)
# Generalized forces
self._gen_forces = np.zeros(6)
def __init__(self,rate):
"""
Constructor of the CrackMap class.
------
Input :
rate : the rate associated to rospy.
Parameters :
PicID : int, associated with the ID of the last depth message
recorded.
cur_PicID : int, associated with the ID of the currently processed
depth message (by the checkCracks function)
CIFrontColor : Camera Info object, containing all the intrinsic parameters
of the camera.
depthMsg : Image Message : ROS message containing the last message published by
the topic /phantomx/crack_image (a depth picture containing the depth of
all the detected cracks on a picture).
depthPic : OpenCV Matrix, containing the last depth picture published by
the topic /phantomx/crack_image.
allCracksInCave : list, containing all the cracks detected since the beginning
of the simulation.
nb_cracks : int, containing the number of cracks detected since the beginning of the
simulation.
dist_center_cracks : float, indicating the minimum 3D distance between the barycenters
of the cracks.
R_rpic2rcam : rotationnal matrix from the picture frame to the camera sensor frame
R_rcam2rpic : rotationnal matrix from the camera sensor frame to the picture frame
buffer : BufferInterface object, stores the last transformation matrices between frames for
a certain time (60 seconds here).
listener : TransformListener object, used to express the coordinates of a pixel from the
camera sensor frame into the cave frame.
"""
self.bridge = CvBridge()
self.PicID = 0
self.cur_PicID = 0
self.CIFrontDepth = None
self.CIFrontColor = None
self.depthMsg = None
self.depthPic = None
self.allCracksInCave = []
self.nb_cracks = len(self.allCracksInCave)
self.dist_center_cracks = 0.5
self.dist_close_cracks = 4.0
#self.current_depthPic = None
self.buffer = tf2.Buffer(rospy.Duration(60)) # prend 60s de tf
self.listener = tf2.TransformListener(self.buffer)
self.CIDtopic = rospy.Subscriber("/phantomx/camera_front/depth/camera_info", CamInfoMSG,self.callbackCIFrontDepth)
self.CIPtopic = rospy.Subscriber("/phantomx/camera_front/color/camera_info", CamInfoMSG,self.callbackCIFrontColor)
rospy.Subscriber("/phantomx/crack_image", ImageMSG,self.callbackDepthPic)
self.rate = rospy.Rate(rate)
self.markerPub = rospy.Publisher('/phantomx/crack_markers', MarkerArray, queue_size=10)
time.sleep(3)
self.cameraModel = image_geometry.PinholeCameraModel()
self.R_rcam2rpic = np.array([[0.0,-1.0,0.0],[0.,0.,-1.0],[1.0,0.0,0.0]])
self.R_rpic2rcam = np.linalg.inv(self.R_rcam2rpic)
while not rospy.is_shutdown():
if (self.depthMsg is not None):
self.checkCracks()
self.displayCracks()
self.rate.sleep()
def __init__(self):
rospy.init_node('robot')
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
self.pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self.zeroTime = rospy.Time()
def main(unused_argv):
vrb = ViveRobotBridge()
# vrb.__init__()
rospy.init_node('vive_listener')
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
# pub = rospy.Publisher('tarpos_pub', geometry_msgs.msg.Transform, queue_size=1)
# _joy_sub = rospy.Subscriber('/vive/controller_LHR_FFFF3F47/joy', Joy, vive_controller_button_callback, queue_size=1)
temp_flag = 0
pre_position = [0, 0, 0]
pre_pose = [1, 0, 0, 0]
temp = [1, 0, 0, 0]
rate = rospy.Rate(100.0)
limits = 0.02
env = Environment(FLAGS.assets_root,
disp=FLAGS.disp,
shared_memory=FLAGS.shared_memory,
hz=480)
task = tasks.names[FLAGS.task]()
task.mode = FLAGS.mode
agent = task.oracle(env)
env.set_task(task)
obs = env.reset()
info = None
ee_pose = ((0.46562498807907104, -0.375, 0.3599780201911926), (0.0, 0.0,
0.0, 1.0))
while not rospy.is_shutdown():
#act = agent.act(obs, info)
#obs, reward, done, info = env.step(act)
env.movep(ee_pose)
if vrb.grasp == 1:
env.ee.activate()
else:
env.ee.release()
try:
trans = tfBuffer.lookup_transform('world',
'controller_LHR_FF777F05',
rospy.Time())
# rospy.loginfo(trans)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.loginfo("error")
rate.sleep()
continue
if vrb.offset_flag == 1:
if temp_flag == 0:
# vrb.offset[0] = trans.transform.translation.x
# vrb.offset[1] = trans.transform.translation.y
# vrb.offset[2] = trans.transform.translation.z
# print(vrb.offset)
pre_position[0] = trans.transform.translation.x
pre_position[1] = trans.transform.translation.y
pre_position[2] = trans.transform.translation.z
pre_pose[0] = trans.transform.rotation.x
pre_pose[1] = trans.transform.rotation.y
pre_pose[2] = trans.transform.rotation.z
pre_pose[3] = trans.transform.rotation.w
else:
msg = geometry_msgs.msg.Transform()
# msg.translation.x = (trans.transform.translation.x-pre_position[0])/10
# msg.translation.y = (trans.transform.translation.y-pre_position[1])/10
# msg.translation.z = (trans.transform.translation.z-pre_position[2])/10
# compute delta distance
msg.translation.x = trans.transform.translation.x - pre_position[
0]
msg.translation.y = trans.transform.translation.y - pre_position[
1]
msg.translation.z = trans.transform.translation.z - pre_position[
2]
# if msg.translation.x >0.02:
# msg.translation.x = 0.02
# if msg.translation.y >0.02:
# msg.translation.y = 0.02
# if msg.translation.z >0.02:
# msg.translation.z = 0.02
# if msg.translation.x <-0.02:
# msg.translation.x = -0.02
# if msg.translation.y <-0.02:
# msg.translation.y = -0.02
# if msg.translation.z <-0.02:
# msg.translation.z = -0.02
if msg.translation.x > limits:
msg.translation.x = limits
if msg.translation.y > limits:
msg.translation.y = limits
if msg.translation.z > limits:
msg.translation.z = limits
if msg.translation.x < -limits:
msg.translation.x = -limits
if msg.translation.y < -limits:
msg.translation.y = -limits
if msg.translation.z < -limits:
msg.translation.z = -limits
print(msg.translation)
# temp[0] = trans.transform.rotation.x
# temp[1] = trans.transform.rotation.y
# temp[2] = trans.transform.rotation.z
# temp[3] = trans.transform.rotation.w
#
#
#
# q = Quaternion(pre_pose) * Quaternion(temp).inverse
#
#
# msg.rotation.x = q.x
# msg.rotation.y = q.y
# msg.rotation.z = q.z
# msg.rotation.w = q.w#
msg.rotation.x = trans.transform.rotation.x
msg.rotation.y = trans.transform.rotation.y
msg.rotation.z = trans.transform.rotation.z
msg.rotation.w = trans.transform.rotation.w
ee_position = list(p.getLinkState(env.ur5, env.ee_tip)[4])
#ee_orientation = p.getLinkState(self.ur5, self.ee_tip)[5]
ee_orientation = (0, 0, 0, 1)
ee_position[0] = ee_position[0] + msg.translation.y
ee_position[1] = ee_position[1] - msg.translation.x
# z axis limit
z_control = ee_position[2] + msg.translation.z
if z_control < 0.02:
z_control = 0.02
ee_position[2] = z_control
ee_pose = (tuple(ee_position), ee_orientation)
# rectified quaternion
# theta_x = np.arcsin(2*(trans.transform.rotation.w*trans.transform.rotation.y
# - trans.transform.rotation.z*trans.transform.rotation.x))
# temp = (trans.transform.rotation.x**2 + trans.transform.rotation.z**2)**0.5
# z_v = trans.transform.rotation.z / temp
# x_v = trans.transform.rotation.x / temp
# msg.rotation.x = x_v * np.sin(theta_x)#0#np.sin(0.5*theta_x)
# msg.rotation.y = 0#y_v * np.sin(theta_x)#0
# msg.rotation.z = z_v * np.sin(theta_x)
# msg.rotation.w = trans.transform.rotation.w
#
# msg.translation.x = 0
# msg.translation.y = 0
# msg.translation.z = 0
#
# msg.rotation.x = 0
# msg.rotation.y = 0
# msg.rotation.z = 0
# msg.rotation.w = 1
print(msg.rotation)
vrb.pub.publish(msg)
pre_position[0] = trans.transform.translation.x
pre_position[1] = trans.transform.translation.y
pre_position[2] = trans.transform.translation.z
temp_flag = vrb.offset_flag
rate.sleep()
def controller(turtlebot_frame, goal_frame):
"""
Controls a turtlebot whose position is denoted by turtlebot_frame,
to go to a position denoted by target_frame
Inputs:
- turtlebot_frame: the tf frame of the AR tag on your turtlebot
- target_frame: the tf frame of the target AR tag
"""
################################### YOUR CODE HERE ##############
#Create a publisher and a tf buffer, which is primed with a tf listener
pub = rospy.Publisher("/yellow/mobile_base/commands/velocity",
Twist,
queue_size=10)
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
br = tf2_ros.TransformBroadcaster()
# Create a timer object that will sleep long enough to result in
# a 10Hz publishing rate
r = rospy.Rate(10) # 10hz
K1 = 0.3
K2 = 1
Ks = [K1, K2]
# Loop until the node is killed with Ctrl-C
while not rospy.is_shutdown():
try:
#print("Before")
rospy.sleep(0.1)
t = TransformStamped()
t.header.stamp = rospy.Time.now()
t.transform.translation.x = 0
t.transform.translation.y = 0
t.transform.translation.z = 0
t.transform.rotation.x = 0
t.transform.rotation.y = 0
t.transform.rotation.z = -0.707
t.transform.rotation.w = 0.707
t.header.frame_id = "left"
t.child_frame_id = "ball"
br.sendTransform(t)
#t.header.frame_id = "test"
trans = tfBuffer.lookup_transform(turtlebot_frame, "left",
rospy.Time())
#print("After")
# Process trans to get your state error
# Generate a control command to send to the robot
#print(trans)
x = trans.transform.translation.x * Ks[0]
theta = trans.transform.translation.y * Ks[1]
#print(turtlebot_frame)
control_command = Twist(Vector3(x, 0, 0), Vector3(0, 0, theta))
#################################### end your code ###############
pub.publish(control_command)
print(control_command)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
print(e)
pass
# Use our rate object to sleep until it is time to publish again
r.sleep()
def depthImageCallback(self, data):
time_start = time.time()
# neck_rad = self.whole_body.joint_positions['head_tilt_joint']
# if neck_rad != -np.radians(45.0):
# self.whole_body.move_to_joint_positions({'head_tilt_joint':-np.radians(45.0)})
try:
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
trans = tf_buffer.lookup_transform("map",
"head_rgbd_sensor_rgb_frame",
rospy.Time(0),
rospy.Duration(1.0))
except Exception as e:
rospy.loginfo("%s", e)
return
depthImage = CvBridge().imgmsg_to_cv2(data, '16UC1')
heightImage = trans.transform.translation.z - np.array(
depthImage) * np.sin(np.radians(45.0)) * 0.001 - self.floor_adjust
heightImage[depthImage < 10.0] = 0.0
if self.use_color:
colorImage = CvBridge().imgmsg_to_cv2(self.colorImageData, 'rgb8')
colorImage[heightImage < self.heightThreshold] = [0, 0, 0]
grayImage = cv2.cvtColor(colorImage, cv2.COLOR_BGR2GRAY)
retval, bw = cv2.threshold(grayImage, 50, 225,
cv2.THRESH_BINARY | cv2.THRESH_OTSU)
else:
depthImage[heightImage < self.heightThreshold] = 0.0
grayImage = np.uint8(depthImage)
retval, bw = cv2.threshold(grayImage, 1, 225, cv2.THRESH_BINARY)
image, contours, hierarchy = cv2.findContours(bw, cv2.RETR_LIST,
cv2.CHAIN_APPROX_NONE)
# s = cv2.cvtColor(colorImage, cv2.COLOR_BGR2RGBA)
# s[heightImage<self.heightThreshold] = [0,0,0,0]
# cv2.imwrite("hoge.png", s)
print "depth time : ", time.time() - time_start
br = tf.TransformBroadcaster()
xywh = np.array([
cv2.boundingRect(c) for c in contours
if 200 < cv2.contourArea(c) < 2000 and len(c) > 0
])
if len(xywh) < 1:
return
if self.use_color:
for x, y, w, h in xywh:
cv2.rectangle(colorImage, (x, y), (x + w, y + h), (0, 255, 0),
2)
elif self.output_flag:
for x, y, w, h in xywh:
cv2.rectangle(depthImage, (x, y), (x + w, y + h), 255, 2)
x_pixel_uncheck = xywh[:, 0] + xywh[:, 2] / 2
y_pixel_uncheck = xywh[:, 1] + xywh[:, 3] / 2
dist_uncheck = depthImage[y_pixel_uncheck, x_pixel_uncheck] * 0.001
check_array = (0.8 < dist_uncheck)
x_pixel = x_pixel_uncheck[check_array]
y_pixel = y_pixel_uncheck[check_array]
dist = dist_uncheck[check_array]
x_deg = width_pixel_deg * (x_pixel - image_width / 2)
y_deg = height_pixel_deg * (y_pixel - image_height / 2)
tf_x = np.sin(np.radians(x_deg)) * dist
tf_y = np.cos(np.radians(y_deg)) * dist
for count, (tf_xi, tf_yi) in enumerate(zip(tf_x, tf_y)):
name = "followeme_" + str(count)
br.sendTransform((tf_xi, 0.0, tf_yi), (0.0, 0.0, 0.0, 1.0),
rospy.Time.now(), name,
"head_rgbd_sensor_rgb_frame")
print "fin time : ", time.time() - time_start
if self.use_color:
self.image_pub.publish(CvBridge().cv2_to_imgmsg(
colorImage, "rgb8"))
elif self.output_flag:
self.image_pub.publish(CvBridge().cv2_to_imgmsg(
depthImage, "16UC1"))
def __init__(self):
rospy.init_node('node_tf_echo')
self._tfBuffer = tf2_ros.Buffer()
self._listener = tf2_ros.TransformListener(self._tfBuffer)
def __init__(self):
action_ns = rospy.get_param('~action_ns', '')
robot_name = rospy.get_param('~robot_name')
self.lock = threading.RLock()
self.publish_rate = rospy.get_param('~publish_rate', 50)
sec = rospy.get_param('~tf_exp_time', 90.0)
self.tf_exp_time = rospy.Duration(sec)
self.human_frame = rospy.get_param('~human_frame_id',
'human_footprint')
self.robot_root_frame = rospy.get_param('~robot_root_frame',
robot_name + '/odom')
drone_goto_action_ns = rospy.get_param(
'~drone_goto_action_ns', '/' + robot_name + '/goto_action')
drone_shape_action_ns = rospy.get_param(
'~drone_followpath_action_ns',
'/' + robot_name + '/followshape_action')
drone_followme_action_ns = rospy.get_param(
'~drone_followme_action_ns', '/' + robot_name + '/followme_action')
self.ray_origin_frame = rospy.get_param('~ray_origin_frame', 'eyes')
self.ray_direction_frame = rospy.get_param('~ray_direction_frame',
'pointer')
self.ray_inverse = rospy.get_param('~ray_inverse', False)
pose_topic = rospy.get_param('~robot_pose_topic',
'/' + robot_name + '/odom/pose/pose')
pose_topic_class, pose_real_topic, pose_eval_func = rostopic.get_topic_class(
pose_topic)
self.robot_pose_msg_eval = pose_eval_func
############ FIXME ############
trigger_topic = rospy.get_param('~trigger_topic',
'/' + robot_name + '/joy/buttons[6]')
trigger_topic_class, trigger_real_topic, trigger_eval_func = rostopic.get_topic_class(
trigger_topic)
self.trigger_msg_eval = trigger_eval_func
# self.trigger_sub = rospy.Subscriber(trigger_real_topic, trigger_topic_class, self.trigger_cb)
self.trigger_val = None
self.last_trigger_val = None
###############################
self.timewindow = rospy.get_param('~timewindow', 5.0)
self.sync_freq = rospy.get_param('~freq', 20.0)
self.sample_size = rospy.get_param('~sample_size',
self.sync_freq * 3.0)
self.residual_threshold = np.radians(
rospy.get_param('~residual_threshold_deg', 3.0))
self.robot_motion_span_min = rospy.get_param('~robot_motion_span_min',
0.20) # 20 cm
if self.timewindow and self.sync_freq:
self.queue_size = int(self.timewindow * self.sync_freq)
# 5.0 seconds at 5 Hz
rospy.loginfo('Max queue size: {}'.format(self.queue_size))
if self.sample_size > self.queue_size:
rospy.loginfo(
'sample_size [{}] is bigger than queue_size [{}]. Setting sample_size = queue_size'
.format(self.sample_size, self.queue_size))
self.sample_size = self.queue_size
else:
rospy.logwarn(
'Either timewindow or queue_size is set to 0. Using unbound queue.'
)
self.queue_size = None
self.deque = collections.deque(maxlen=self.queue_size)
self.relloc_deque = collections.deque(
maxlen=self.sync_freq *
1.0) # Use 1s of relloc data to trigger selection
self.robot_pose_msg = None
self.robot_sub = rospy.Subscriber(pose_real_topic, pose_topic_class,
self.robot_pose_cb)
self.is_valid_pub = rospy.Publisher('is_relloc_valid',
Bool,
queue_size=10)
self.initial_guess = np.array([0, 0, 0, 0])
self.reset_state()
self.tf_buff = tf2_ros.Buffer()
self.tf_ls = tf2_ros.TransformListener(self.tf_buff)
self.tf_br = tf2_ros.TransformBroadcaster()
self.drone_goto_client = actionlib.SimpleActionClient(
drone_goto_action_ns, GoToAction)
rospy.loginfo('Waiting for ' + drone_goto_action_ns)
self.drone_goto_client.wait_for_server()
# self.drone_shape_client = actionlib.SimpleActionClient(drone_shape_action_ns, FollowShapeAction)
# rospy.loginfo('Waiting for ' + drone_shape_action_ns)
# self.drone_shape_client.wait_for_server()
self.drone_followme_client = actionlib.SimpleActionClient(
drone_followme_action_ns, FollowMeAction)
rospy.loginfo('Waiting for ' + drone_followme_action_ns)
self.drone_followme_client.wait_for_server()
self.relloc_server = actionlib.SimpleActionServer(
action_ns + '/relloc_action', MotionRellocContAction,
self.execute_relloc, False)
self.relloc_server.start()
self.relloc_cont_server = actionlib.SimpleActionServer(
action_ns + '/relloc_cont_action', MotionRellocContAction,
self.execute_relloc_cont, False)
self.relloc_cont_server.start()
if not os.path.isdir(os.path.join(save_dir, str(i)+'_npy')):
os.makedirs(os.path.join(save_dir, str(i)+'_npy', 'data'))
os.makedirs(os.path.join(save_dir, str(i)+'_npy', 'gt'))
exit()
'''
origin_tag = '0'
all_tags = [
'0', '1', '2', '3', '4', '5', '16', '7', '8', '9', '10', '11', '17', '14',
'15'
]
rospy.init_node("data_node", anonymous=True)
buf = tf.Buffer()
ls = tf.TransformListener(buf)
gt_dict = dict()
data_dict = dict()
def data_synced(tag, tag_odom):
global origin_tag, data_dict, gt_dict
if len(tag.detections) > 0:
if len(tag.detections) > 1:
detected_tag = sort_l2(tag.detections)
else:
detected_tag = tag.detections[0]
def __init__(self):
rospy.init_node("show_world_model_objects")
self.marker_publisher = rospy.Publisher(
"visualization_world_model_markers", Marker, queue_size=10)
# object properties
self.ball_diameter = 0.17
self.lifetime_ball = rospy.get_param('behavior/body/ball_lost_time')
self.lifetime_goal = rospy.get_param('behavior/body/goal_lost_time')
self.lifetime_ball_kick_area = 1
self.post_diameter = 0.15
self.post_height = 1.10
position = Pose()
position.position.x = 1.0
position.position.y = 1.0
position.position.z = 0.0
position.orientation.w = 1.0
position.orientation.x = 0.0
position.orientation.y = 0.0
position.orientation.z = 0.0
self.base_footprint_frame = rospy.get_param('~base_footprint_frame',
'base_footprint')
# init ball marker
self.marker_ball = Marker() # type: Marker
self.marker_ball.id = 0
self.marker_ball.type = Marker.SPHERE
self.ball_pose = Pose()
scale = Vector3(self.ball_diameter, self.ball_diameter,
self.ball_diameter)
self.marker_ball.scale = scale
self.ball_color = ColorRGBA()
self.ball_color.a = 0.0
self.ball_color.g = 1.0
self.marker_ball.color = self.ball_color
self.marker_ball.pose = position
self.marker_ball.lifetime = rospy.Duration(self.lifetime_ball)
# init goal markers
self.marker_goal_first_post = Marker() # type:Marker
self.marker_goal_first_post.id = 1
self.marker_goal_first_post.type = Marker.CYLINDER
self.goal_post_pose = Pose()
scale = Vector3(self.post_diameter, self.post_diameter,
self.post_height)
self.marker_goal_first_post.scale = scale
self.post_left_color = ColorRGBA()
self.post_left_color.a = 0.0
self.post_left_color.g = 1.0
self.marker_goal_first_post.color = self.post_left_color
self.marker_goal_first_post.pose = position
self.marker_goal_first_post.lifetime = rospy.Duration(
self.lifetime_goal)
self.marker_goal_second_post = Marker() # type:Marker
self.marker_goal_second_post.id = 2
self.marker_goal_second_post.type = Marker.CYLINDER
self.goal_post_pose = Pose()
scale = Vector3(self.post_diameter, self.post_diameter,
self.post_height)
self.marker_goal_second_post.scale = scale
self.post_right_color = ColorRGBA()
self.post_right_color.a = 0.0
self.post_right_color.b = 1.0
self.marker_goal_second_post.color = self.post_right_color
self.marker_goal_second_post.pose = position
self.marker_goal_second_post.lifetime = rospy.Duration(
self.lifetime_goal)
# init ball kick area markers
self.marker_kick_area = Marker()
self.marker_kick_area.id = 3
self.marker_kick_area.type = Marker.LINE_STRIP
self.marker_kick_area.scale.x = 0.01
self.kick_area_color = ColorRGBA()
self.kick_area_color.a = 0.5
self.kick_area_color.r = 0.8
self.kick_area_color.g = 0.8
self.kick_area_color.b = 0.8
self.marker_kick_area.color = self.kick_area_color
self.marker_kick_area.lifetime = rospy.Duration(
self.lifetime_ball_kick_area)
self.marker_kick_area.pose.orientation.w = 1.0
# init subscribers
rospy.Subscriber("debug/viz_ball",
PointStamped,
self.ball_cb,
queue_size=10)
rospy.Subscriber("debug/viz_goal",
PoseWithCertaintyArray,
self.goal_cb,
queue_size=10)
rospy.Subscriber("debug/viz_ball_kick_area",
String,
self.kick_area_cb,
queue_size=10)
# load kick area info
kick_max_x = rospy.get_param('behavior/body/kick_max_x')
kick_max_y = rospy.get_param('behavior/body/kick_max_y')
kick_min_x = rospy.get_param('behavior/body/kick_min_x')
kick_min_y = rospy.get_param('behavior/body/kick_min_y')
self.kick_area_info = [kick_max_x, kick_max_y, kick_min_x, kick_min_y]
# init tf listener for ball_kick_area_viz
self.tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(self.tfBuffer)
self.last_received_kick_info = rospy.Time.now()
self.kick_area()
rospy.spin()
def compute_grasps_handle(req):
tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0))
tf_listener = tf2_ros.TransformListener(tf_buffer)
print("Request for Grasp compute recieved by graspnet!!")
#return AddTwoIntsResponse(req.a + req.b)
## Subscribers ##
#camera_info_topic = '/panda/camera/depth/camera_info'
#depth_img_topic = '/panda/camera/depth/image_raw'
#rgb_img_topic ='/panda/camera/color/image_raw'
cloud_topic = '/transformed_cloud_camLink'
cloud_msg = rospy.wait_for_message(cloud_topic, PointCloud2)
pc = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(cloud_msg)
pc_colors=100*np.ones((pc.shape[0],3))
## output data info ##
print('Total number of points: %s', pc.shape)
print('HighestPoint %s', np.nanmax(pc[:,2]))
print('LowestPoint %s', np.nanmin(pc[:,2]))
print('Farthest Point %s', np.nanmax(pc[:,0]))
print('Nearest Point %s', np.nanmin(pc[:,0]))
#print('Total number of points above the table: %s', pc.shape)
# Smoothed pc comes from averaging the depth for 10 frames and removing
# the pixels with jittery depth between those 10 frames.
#object_pc = data['smoothed_object_pc']
#points_obj = xyzrgb_array_to_pointcloud2(object_pc, pc_colors, stamp = rospy.Time.now(), frame_id = 'world')
#pub2.publish(points_obj)
## Segment out the points that fit the table plane
cloud_seg = pcl_helper.plane_seg(pc)
#cloud_arr = cloud_seg.to_array()
#cloud_colors_arr =100*np.ones((cloud_arr.shape[0],3))
clc = cloud_clustering(cloud_seg)
clc.euclid_cluster()
cloud = clc.clusters[0] # Run grasp-detection only on the closest cloud cluster
points = cloud.to_array()
cloud_colors =100*np.ones((points.shape[0],3))
## Grasp-Estimation ##
latents = estimator.sample_latents()
generated_grasps, generated_scores, _ = estimator.predict_grasps(
sess,
points,
latents,
num_refine_steps=cfg.num_refine_steps,
)
## Sorting grasps ##
scores_np = np.asarray(generated_scores)
sorting = np.argsort(-scores_np) #Negative sign is for descending-order sorting
sorted_scores = scores_np[sorting]
grasps_np = np.asarray(generated_grasps)
sorted_grasps = grasps_np[sorting]
print('Total generated grasps '+str(len(sorted_grasps.tolist())))
filtered_grasps, filtered_scores, grasp_msgs = filter_grasps(sorted_grasps)
print('Filtered grasps '+str(len(filtered_grasps.tolist())))
try:
tf_stamped = tf_buffer.lookup_transform('world', 'panda_camera_link' , rospy.Time(0))
except Exception as inst:
exc_type, exc_obj, exc_tb = sys.exc_info()
print('exception: '+str(inst)+' in '+ str(exc_tb.tb_lineno))
# Transform the grasps to world frame - which is the ref. frame for Panda robot
transformed_grasps = transform_poses(filtered_grasps.tolist(),tf_stamped)
for i, grasp in enumerate(transformed_grasps):
if grasp.position.z < 0.45: #predefined table height
transformed_grasps.pop(i)
print('Transformed grasps above the table '+str(len(transformed_grasps)))
points_obj = pcl_helper.xyzrgb_array_to_pointcloud2(points, cloud_colors, stamp = rospy.Time.now(), frame_id = 'panda_camera_link')
pub2.publish(points_obj)
pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id='world'), poses = transformed_grasps))
return PoseArray(header=Header(stamp=rospy.Time.now(), frame_id='world'), poses = transformed_grasps)
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)
def __init__(self, yaml_config_file="turtlebot3_world_mapping.yaml"):
"""
This Task Env is designed for having two TurtleBot3 robots in the turtlebot3 world closed room with columns.
It will learn how to move around without crashing.
"""
# This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
ros_ws_abspath = rospy.get_param("/turtlebot3/ros_ws_abspath", None)
if os.environ.get('ROS_WS') != None:
ros_ws_abspath = os.environ.get('ROS_WS')
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(rospackage_name="openai_ros",
rel_path_from_package_to_file="src/openai_ros/task_envs/turtlebot3_my_envs/config",
yaml_file_name=yaml_config_file)
# Depending on which environment we're in, decide to launch Gazebo with or without GUI.
gazebo_launch_file = "start_empty_tb3_world.launch"
if os.environ.get('ENV') == 'deploy' or os.environ.get('ENV') == 'dev-no-gazebo':
gazebo_launch_file = "start_empty_tb3_world_no_gui.launch"
ROSLauncher(rospackage_name="coop_mapping",
launch_file_name=gazebo_launch_file,
ros_ws_abspath=ros_ws_abspath)
# Here we will add any init functions prior to starting the MyRobotEnv
super(TurtleBot3WorldMapping2RobotsEnv, self).__init__(ros_ws_abspath,
ros_launch_file_package="coop_mapping",
ros_launch_file_name="spawn_2_robots.launch")
### ACTIONS
# Only variable needed to be set here
self.number_actions = rospy.get_param('/turtlebot3/n_actions')
self.number_robots = len(self.robot_namespaces) # should be 2
# 3x3 possible actions (a % 3 -> robot 1 action, a / 3 -> robot 2 action)
self.action_space = spaces.Discrete(
pow(self.number_actions, self.number_robots))
# We set the reward range, which is not compulsory but here we do it.
self.reward_range = (-np.inf, np.inf)
### OBSERVATIONS
self.linear_forward_speed = rospy.get_param(
'/turtlebot3/linear_forward_speed')
self.linear_turn_speed = rospy.get_param(
'/turtlebot3/linear_turn_speed')
self.angular_speed = rospy.get_param('/turtlebot3/angular_speed')
self.init_linear_forward_speed = rospy.get_param(
'/turtlebot3/init_linear_forward_speed')
self.init_linear_turn_speed = rospy.get_param(
'/turtlebot3/init_linear_turn_speed')
self.new_ranges = rospy.get_param('/turtlebot3/new_ranges')
self.min_range = rospy.get_param('/turtlebot3/min_range')
self.max_laser_value = rospy.get_param('/turtlebot3/max_laser_value')
self.min_laser_value = rospy.get_param('/turtlebot3/min_laser_value')
"""
An observation is a MultiDiscrete element, with 4 components.
1. LaserScan rays component with R integers, where R is the number of laser scan
rays we are using for observation (one for each robot).
- Each value represents the distance to an obstacle rounded to the nearest integer (in meteres).
2. Position information with 2 integers (x, y) (one for each robot).
- Each value represents the position of the robot along a normalized axis, rouned to the nearest integer.
3. Rotation information with 1 integer (rotation along the z axis) (one for each robot).
- Each value represents the orientation in a normalized scale, rounded to the nearest integer.
4. Simplified map exploration with NxN integers, where N is the dimension of the matrix
that portrays the level of exploration in the map (one for BOTH robots).
- Each value represents the average number of pixels explored (-1 is unexplored, 1 is explored).
The value is normalized and then rounded to the nearest integer.
"""
# We create two arrays based on the binary values that will be assigned
# In the discretization method.
laser_scan = self.laser_scan[self.robot_namespaces[0]]
num_laser_readings = len(laser_scan.ranges)/self.new_ranges
high = np.full((num_laser_readings), self.max_laser_value)
low = np.full((num_laser_readings), self.min_laser_value)
self.position_num_discrete_values = 40
self.rotation_num_discrete_values = 6
self.simplified_grid_dimension = 4
self.simplified_grid_num_discrete_values = 40
laser_scan_component_shape = [
round(self.max_laser_value)] * (self.new_ranges * self.number_robots)
position_component_shape = [
self.position_num_discrete_values] * (2 * self.number_robots)
rotation_component_shape = [
self.rotation_num_discrete_values] * (1 * self.number_robots)
map_exploration_component_shape = [
self.simplified_grid_num_discrete_values] * self.simplified_grid_dimension * self.simplified_grid_dimension
multi_discrete_shape = list(itertools.chain(laser_scan_component_shape,
position_component_shape,
rotation_component_shape,
map_exploration_component_shape))
self.observation_space = spaces.MultiDiscrete(
multi_discrete_shape)
rospy.loginfo("ACTION SPACES TYPE===>"+str(self.action_space))
rospy.loginfo("OBSERVATION SPACES TYPE===>" +
str(self.observation_space))
# Rewards
self.no_crash_reward_points = rospy.get_param(
"/turtlebot3/no_crash_reward_points")
self.crash_reward_points = rospy.get_param(
"/turtlebot3/crash_reward_points")
self.exploration_multi_factor = rospy.get_param(
"/turtlebot3/exploration_multi_factor")
self.cumulated_steps = 0.0
# Init dictionary for both robots actions
self.last_action = {}
# Set the logging system
rospack = rospkg.RosPack()
pkg_path = rospack.get_path('coop_mapping')
# Control variables for launching nodes from .launch files
self._gmapping_launch_file = pkg_path + os.path.sep + \
'launch' + os.path.sep + 'init_2_robots_mapping.launch'
self._gmapping_running = False
self._gmapping_launch = None
self._map_merge_launch_file = pkg_path + os.path.sep + \
'launch' + os.path.sep + 'init_2_robots_multi_map_merge.launch'
self._map_merge_running = False
self._map_merge_launch = None
self._hector_saver_launch_file = pkg_path + os.path.sep + \
'launch' + os.path.sep + 'init_2_robots_hector_saver.launch'
self._hector_saver_running = False
self._hector_saver_launch = None
# Variables for map comparison
self.map_data = None
self._num_white_pixels_to_explore = get_number_of_almost_white_pixels(self.actual_map_file)
# The minimum difference that has been observed
self.current_min_map_difference = None
# The thresholds to calculate map exploration
self.threshold_occupied = 65
self.threshold_free = 25
# The area in pixels that has been explored
self.previous_max_explored_area = None
self.current_max_explored_area = None
# Start subscriber to /map to save it to file
self._map_file_name = "/tmp/ros_merge_map"
self._map_updated_after_action = False
rospy.Subscriber('map', OccupancyGrid, self._map_callback)
# Logging episode information
self._first_episode = True
# TF listener to get robots position
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
def __init__(self, node_name):
self.debug = False
# Initialize DTROS Parent Class
super(EncoderLocalization, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh_name = rospy.get_namespace().strip("/")
self.log(f"Using vehicle name {self.veh_name}")
# Get Baseline and Radius
self.baseline, self.radius = self.get_calib_params()
self.log(
f'Parameters: baseline = {self.baseline}, radius = {self.radius}')
# Initialize Encoder Variables
self.encoder_received = False
self.encoder_resolution = 135
self.initialised_ticks_left = False
self.total_ticks_left = 0
self.initial_ticks_left = 0
self.wheel_distance_left = 0.0
self.initialised_ticks_right = False
self.total_ticks_right = 0
self.initial_ticks_right = 0
self.wheel_distance_right = 0.0
# Initialize Estimation Variables (in map reference frame)
self.theta = np.pi
self.x = 1.0
self.y = 0.0
# Initialize Transform Message
self.transform_msg = TransformStamped() #published in self.run()
self.transform_msg.header.frame_id = "map" #parent frame
self.transform_msg.child_frame_id = "encoder_baselink"
self.transform_msg.transform.translation.z = 0.0 # project to ground
# Initialize TF Broadcaster
self.broadcaster = tf2_ros.TransformBroadcaster()
# Initialize TF Listener
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
# Initialize Publishers
pub_topic = f'/{self.veh_name}/encoder_localization/transform'
self.pub_transform = rospy.Publisher(pub_topic,
TransformStamped,
queue_size=10)
# Initialize Subscribers
left_encoder_topic = f'/{self.veh_name}/left_wheel_encoder_node/tick'
right_encoder_topic = f'/{self.veh_name}/right_wheel_encoder_node/tick'
self.sub_encoder_ticks_left = rospy.Subscriber(
left_encoder_topic,
WheelEncoderStamped,
self.cb_encoder_to_transform,
callback_args="left")
self.sub_encoder_ticks_right = rospy.Subscriber(
right_encoder_topic,
WheelEncoderStamped,
self.cb_encoder_to_transform,
callback_args="right")
# Initialize Services
self.serv_reset = rospy.Service(
f'/{self.veh_name}/encoder_localization/reset', Trigger,
self.reset)
self.serv_update_map_frame = rospy.Service(
f'/{self.veh_name}/encoder_localization/update_map_frame', Trigger,
self.update_map_frame)
# Broadcast and publish initial transform
self.latest_timestamp = rospy.Time.now()
self.update_transform()
self.broadcaster.sendTransform(self.transform_msg)
self.pub_transform.publish(self.transform_msg)
self.log("Initialized.")
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()
# ========== TF Lister ========== #
tf_buffer = tf2_ros.Buffer()
tf_listner = tf2_ros.TransformListener(tf_buffer)
get_tf_flg = False
# Get target TF
while not get_tf_flg :
try :
trans = tf_buffer.lookup_transform('world', 'ar_marker', 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 + 0.03
target_pose.position.z = trans.transform.translation.z + 0.42
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(1)
# 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.03
target_pose.position.z = trans.transform.translation.z + 0.283
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(1.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.03
target_pose.position.z = trans.transform.translation.z + 0.42
target_pose.orientation = target_pose.orientation
print target_pose
arm.set_pose_target(target_pose)
arm.go()
arm.clear_pose_targets()
# Go to Home Position
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.42
target_pose.orientation = target_pose.orientation
arm.set_pose_target(target_pose)
arm.go()
arm.clear_pose_targets()
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.30
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(2)
get_tf_flg = True
target_pose.position.x = 0.503
target_pose.position.y = 0
target_pose.position.z = 0.6563
target_pose.orientation.x = 0
target_pose.orientation.y = 0
target_pose.orientation.z = 0
target_pose.orientation.w = 1
arm.set_pose_target(target_pose)
arm.go()
arm.clear_pose_targets()
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) :
continue
def __init__(self):
map_frame = rospy.get_param('~map_frame', 'map') # map frame_id
odom_frame = rospy.get_param('~odom_frame', 'odom')
meas_model_as = rospy.get_param('~mbes_as',
'/mbes_sim_server') # map frame_id
auv_odom_top = rospy.get_param("~odometry_topic", '/odom')
auv_mbes_top = rospy.get_param("~mbes_pings_topic", '/mbes')
auv_exp_mbes_top = rospy.get_param("~expected_mbes_topic",
'/expected_mbes')
pf_pose_top = rospy.get_param("~average_pose_topic", '/avg_pose')
self.auv_mbes = message_filters.Subscriber(auv_mbes_top, PointCloud2)
self.exp_mbes = message_filters.Subscriber(auv_exp_mbes_top,
PointCloud2)
self.auv_pose = message_filters.Subscriber(auv_odom_top, Odometry)
# self.pf_pose = message_filters.Subscriber(pf_pose_top, PoseWithCovarianceStamped)
self.ts = message_filters.ApproximateTimeSynchronizer(
[self.auv_mbes, self.exp_mbes, self.auv_pose],
10,
slop=10.0,
allow_headerless=False)
# 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 = self.matrix_from_tf(mbes_tf)
m2o_tf = tfBuffer.lookup_transform(map_frame, odom_frame,
rospy.Time(0),
rospy.Duration(20.))
self.m2o_mat = self.matrix_from_tf(m2o_tf)
rospy.loginfo("Transforms locked - Car detector node")
except:
rospy.loginfo(
"ERROR: Could not lookup transform from base_link to mbes_link"
)
# Register cb after tf is locked
self.ts.registerCallback(self.pingCB)
plt.ion()
plt.show()
self.scale = 1
self.max_height = 100 # TODO: this should equal the n beams in ping
self.new_msg = False
first_msg = True
self.waterfall = []
while not rospy.is_shutdown():
if self.new_msg:
# Blob detection to find the car on waterfall image
#Visualize
if len(self.waterfall) == self.max_height:
waterfall_detect = self.car_detection(
np.array(self.waterfall), self.scale)
plt.imshow(np.array(waterfall_detect),
norm=plt.Normalize(0., 60.),
cmap='gray',
aspect='equal')
else:
plt.imshow(np.array(self.waterfall),
norm=plt.Normalize(0., 60.),
cmap='gray',
aspect='equal')
if first_msg:
first_msg = False
plt.colorbar()
plt.title("Bathymetry difference (m)")
plt.pause(0.01)
self.new_msg = False
def __init__(self):
self.pose_cam_1_sub = message_filters.Subscriber(
POSE_CAM_1_TOPIC, CustomMarkerArray)
self.pose_cam_2_sub = message_filters.Subscriber(
POSE_CAM_2_TOPIC, CustomMarkerArray)
self.pose_cam_4_sub = message_filters.Subscriber(
POSE_CAM_4_TOPIC, CustomMarkerArray)
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
self.pub = rospy.Publisher(FUSION_POSES_TOPIC,
MarkerArray,
queue_size=10)
self.check_cam_1_pub = rospy.Publisher(FUSION_CHECK_CAM_1_POSES_TOPIC,
MarkerArray,
queue_size=10)
self.check_cam_2_pub = rospy.Publisher(FUSION_CHECK_CAM_2_POSES_TOPIC,
MarkerArray,
queue_size=10)
self.check_cam_4_pub = rospy.Publisher(FUSION_CHECK_CAM_4_POSES_TOPIC,
MarkerArray,
queue_size=10)
self.sync = message_filters.ApproximateTimeSynchronizer(
[self.pose_cam_1_sub, self.pose_cam_2_sub, self.pose_cam_4_sub],
10, 0.1)
self.sync.registerCallback(self.poses_callback)
self.occluded_points_cam_2 = [4, 6, 8]
self.occluded_points_cam_4 = [3, 5, 7]
# Kalman filter parameters
f = KalmanFilter(dim_x=6, dim_z=3)
self.keypoints = 13
self.initialized = [False for k in range(self.keypoints)]
self.dt = 1 / 15.0 #15 fps
f.inv = np.linalg.pinv
# State transition matrix
f.F = np.array([[1., 0., 0., self.dt, 0., 0.],
[0., 1., 0., 0., self.dt, 0.],
[0., 0., 1., 0., 0., self.dt],
[0., 0., 0., 1., 0., 0.], [0., 0., 0., 0., 1., 0.],
[0., 0., 0., 0., 0., 1.]])
# Measurement function
f.H = np.array([[1., 0., 0., 0., 0., 0.], [0., 1., 0., 0., 0., 0.],
[0., 0., 1., 0., 0., 0.]])
# Process uncertainty/noise
f.Q = np.identity(6) * 1e-4
# Measurement uncertainty/noise
self.min_cov = 0.0025
self.max_cov = 0.04
self.steepness = 20
self.steep_point = 0.6
f.R = np.identity(3) * (self.min_cov + self.max_cov) / 2
f.R[2, 2] = 0.05
# Distance threshold
if USE_MAHALANOBIS:
self.distance_threshold = 60
else:
self.distance_threshold = 0.4
# Covariance matrix
f.P = np.identity(6) * 100
# Create the KalmanFilter array
self.kf_array = [copy.deepcopy(f) for kp in range(self.keypoints)]
# Last-update time array
self.time_since_update = [time.time() for kp in range(self.keypoints)]
self.time_threshold = 0.25 # in secs
# Auxiliar variables (for visualization)
self.cam_1_check_covariances = [
np.zeros((3, 3)) for kp in range(self.keypoints)
]
self.cam_2_check_covariances = [
np.zeros((3, 3)) for kp in range(self.keypoints)
]
self.cam_4_check_covariances = [
np.zeros((3, 3)) for kp in range(self.keypoints)
]
def setup(self):
self.tfBuffer = tf2_ros.Buffer()
self.tfListener = tf2_ros.TransformListener(self.tfBuffer)
def __init__(self):
action_ns = rospy.get_param('~action_ns', '')
# Monitor manual control and cancel any actions if user presses any button
joy_safety_topic = rospy.get_param('~joy_safety_topic', '/drone/joy')
self.distance_threshold = rospy.get_param('~distance_threshold', 0.10)
self.yaw_threshold = rospy.get_param('~yaw_threshold',
math.pi / 180.0 * 10.0)
self.robot_desired_pose_topic = rospy.get_param(
'~robot_desired_pose_topic', 'desired_pose')
robot_odom_topic = rospy.get_param('~robot_odom_topic', '/drone/odom')
robot_state_topic = rospy.get_param('~robot_state_topic',
'/drone/flight_state')
takeoff_service = rospy.get_param('~takeoff_service',
'/drone/safe_takeoff')
land_service = rospy.get_param('~land_service', '/drone/safe_land')
feedback_service = rospy.get_param('~feedback_service',
'/drone/give_feedback')
target_source_topic = rospy.get_param('~target_source_topic',
'/drone/target_source')
set_pose_topic = rospy.get_param('~set_pose_topic', '/drone/set_pose')
set_xy_service = rospy.get_param('~set_xy_service', '/drone/set_xy')
self.robot_desired_pose = PoseStamped()
self.robot_current_pose = PoseStamped()
self.tf_buff = tf2_ros.Buffer()
self.tf_ls = tf2_ros.TransformListener(self.tf_buff)
# Action client to self, see below
self.goto_client = actionlib.SimpleActionClient(
action_ns + '/goto_action', GoToAction)
# Action servers
self.goto_server = ActionHub.add_server(action_ns + '/goto_action',
GoToAction, self.execute_goto,
False)
self.goto_server.start()
# self.shape_server = ActionHub.add_server(action_ns + '/followshape_action', FollowShapeAction, self.execute_shape, False)
# self.shape_server.start()
self.precise_shape_server = ActionHub.add_server(
action_ns + '/precise_shape_action', FollowShapeAction,
self.execute_precise_shape, False)
self.precise_shape_server.start()
self.takeoff_server = ActionHub.add_server(
action_ns + '/takeoff_action', EmptyAction, self.execute_takeoff,
False)
self.takeoff_server.start()
self.land_server = ActionHub.add_server(action_ns + '/land_action',
EmptyAction, self.execute_land,
False)
self.land_server.start()
self.feedback_server = ActionHub.add_server(
action_ns + '/feedback_action', EmptyAction, self.execute_feedback,
False)
self.feedback_server.start()
self.followme_server = ActionHub.add_server(
action_ns + '/followme_action', FollowMeAction,
self.execute_followme, False)
self.followme_server.start()
self.waypoints_server = ActionHub.add_server(
action_ns + '/waypoints_action', WaypointsAction,
self.execute_waypoints, False)
self.waypoints_server.start()
self.reset_odom_server = ActionHub.add_server(
action_ns + '/reset_odom_action', EmptyAction,
self.execute_reset_odom, False)
self.reset_odom_server.start()
self.pub_desired_pose = rospy.Publisher(self.robot_desired_pose_topic,
PoseStamped,
queue_size=10)
self.sub_odom = rospy.Subscriber(robot_odom_topic, Odometry,
self.robot_odom_cb)
self.takeoff_svc = rospy.ServiceProxy(takeoff_service,
std_srvs.srv.Trigger)
self.land_svc = rospy.ServiceProxy(land_service, std_srvs.srv.Trigger)
self.feedback_svc = rospy.ServiceProxy(feedback_service,
std_srvs.srv.Trigger)
self.pub_target_source = rospy.Publisher(target_source_topic,
TargetSource,
queue_size=1)
self.pub_set_pose = rospy.Publisher(set_pose_topic, Pose, queue_size=1)
self.set_xy_service = rospy.ServiceProxy(set_xy_service, SetXY)
self.last_known_flight_state = FlightState.Landed #None
self.sub_flight_state = rospy.Subscriber(robot_state_topic,
FlightState,
self.flight_state_cb)
self.sub_joy_safety = rospy.Subscriber(joy_safety_topic, Joy,
self.joy_safety_cb)
# self.ros_thread = threading.Thread(target=self.run)
# self.ros_thread.start()
rospy.wait_for_service(takeoff_service)
rospy.wait_for_service(land_service)
rospy.wait_for_service(feedback_service)
#!/usr/bin/env python
import rospy
import math
from geometry_msgs.msg import Point,PointStamped,PoseStamped
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from actionlib_msgs.msg import *
import actionlib
import tf2_ros
import tf2_geometry_msgs
import utm
rospy.init_node('GPS_goals')
tf_buffer = tf2_ros.Buffer(rospy.Duration(10))
tf_listener = tf2_ros.TransformListener(tf_buffer)
client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
client.wait_for_server()
gps_pub = rospy.Publisher('/GPS_odom_point', PointStamped, queue_size=10, latch=True)
utm_point = PointStamped()
utm_point.header.frame_id = "utm"
odom_point = PointStamped()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "odom"
goal.target_pose.pose.orientation.x = 0
goal.target_pose.pose.orientation.y = 0
goal.target_pose.pose.orientation.z = 0
def main():
global tfBuffer, tfListener, stepCounter
global ROBOT_NAME, RIGHT_FOOT_FRAME_NAME, LEFT_FOOT_FRAME_NAME
global web
web = addasteppages.Pages()
thread.start_new_thread(web.run, ())
try:
rospy.init_node('AddAStep')
if not rospy.has_param('/ihmc_ros/robot_name'):
rospy.logerr("Missing parameter '/ihmc_ros/robot_name'")
else:
ROBOT_NAME = rospy.get_param('/ihmc_ros/robot_name')
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)
fss_name = "/ihmc_ros/{0}/output/footstep_status".format(ROBOT_NAME)
fsp_name = "/ihmc_ros/{0}/control/footstep_list".format(ROBOT_NAME)
atp_name = "/ihmc_ros/{0}/control/arm_trajectory".format(ROBOT_NAME)
ctp_name = "/ihmc_ros/{0}/control/chest_trajectory".format(ROBOT_NAME)
pose_name = "/ihmc_ros/{0}/output/robot_pose".format(ROBOT_NAME)
if rospy.has_param(right_foot_frame_parameter_name) and rospy.has_param(left_foot_frame_parameter_name):
RIGHT_FOOT_FRAME_NAME = rospy.get_param(right_foot_frame_parameter_name)
LEFT_FOOT_FRAME_NAME = rospy.get_param(left_foot_frame_parameter_name)
footStepStatusSubscriber = rospy.Subscriber( fss_name, FootstepStatusRosMessage, recievedFootStepStatus)
robotPoseSubscriber = rospy.Subscriber( pose_name, Odometry, recievedRobotPose )
footStepListPublisher = rospy.Publisher( fsp_name, FootstepDataListRosMessage, queue_size=1)
armTrajectoryPublisher = rospy.Publisher( atp_name, ArmTrajectoryRosMessage, queue_size=1)
chestTrajectoryPublisher = rospy.Publisher( ctp_name, ChestTrajectoryRosMessage, queue_size=1)
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
rate = rospy.Rate(10) # 10hz
time.sleep(1)
# make sure the simulation is running otherwise wait
if footStepListPublisher.get_num_connections() == 0:
rospy.loginfo('waiting for subsciber...')
while footStepListPublisher.get_num_connections() == 0:
rate.sleep()
stepCounter = 0
backsent = armsent = footsent = False
backnote = armnote = footnote = 'Wait '
while not rospy.is_shutdown():
done, curtim = monitor()
print(backnote,footnote,armnote)
if curtim > 28.0 and not backsent:
footStepMsg,backnote = createBackStepCmd()
footStepListPublisher.publish(footStepMsg)
backsent = True
print('\a')
if curtim > 35.0 and not armsent:
footStepMsg,footnote = createFootStepCmd()
armMsgL, armMsgR, armnote = createArmCmd()
armTrajectoryPublisher.publish(armMsgL)
rospy.sleep(rospy.Duration(0,10000000))
armTrajectoryPublisher.publish(armMsgR)
chestMsg, _ = createChestCmd()
chestTrajectoryPublisher.publish(chestMsg)
#armTrajectoryPublisher.publish(armMsgR)
armsent = True
print('\a')
if curtim > 40.0 and not footsent:
footStepListPublisher.publish(footStepMsg)
footsent = True
print('\a')
print('\a')
if (done):
print('\a')
print('\a')
return
rate.sleep()
except rospy.ROSInterruptException:
pass
import tf2_geometry_msgs
import tf2_ros
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('points_to_xyz_with_moveit_node')
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("manipulator")
tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) # tf buffer length
tf_listener = tf2_ros.TransformListener(tf_buffer)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
rospy.sleep(1)
group.set_planning_time(10)
group.set_joint_value_target([0, 0, 0, 0, -pi / 2, 0])
group.go()
def __init__(self):
rospy.init_node("logger", anonymous=True)
# if true -> show plots on shutdown
self.show_plots = rospy.get_param("~show_plots", False)
# if true -> save data with the start time in the name
self.track_time = rospy.get_param("~track_time", False)
# path to the output data folder
self.module_path = rospy.get_param("~output_file")
# output folder name
self.output_folder = rospy.get_param("~output_folder")
self.parent_frame = rospy.get_param("~parent_frame", 'odom')
self.robot_frame = rospy.get_param("~robot_frame", 'base_link')
self.kinetic_model_frame = rospy.get_param("~kinetic_model_frame",
'model_link')
self.nn_model_frame = rospy.get_param("~nn_model_frame",
'nn_model_link')
self.timeout = int(rospy.get_param("~timeout", 0))
self.module_path = self.module_path + self.output_folder
if self.track_time:
self.module_path += '/' + str(
datetime.now().strftime('%Y-%m-%d-%H:%M:%S'))
# declare tf buffer and listener for working with TF transformations
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# node init time
self.init_time = None
#
self.prev_time = None
# flag for the first callback (used in Tf callback)
self.first_tick = True
self.fisrt_fill_state = True
# container for trajectory (/path) fron path_publisher node
self.trajectory = {'x': [], 'y': []}
# container for /cmd_vel (input control) from path_follower node
self.control = {'x': [], 'yaw': []}
# container for robot state
self.robot_state = {'x': [], 'y': [], 'yaw': [], 'v': [], 'w': []}
# container for kinetic model state
self.kinetic_model_state = {
'x': [],
'y': [],
'yaw': [],
'v': [],
'w': []
}
# container for nn model state
self.nn_model_state = {'x': [], 'y': [], 'yaw': [], 'v': [], 'w': []}
# time spent running the simulator (for automated tests)
self.delta_time = {'dt': []}
self.time = {'t': []}
self.time_spent = 0
self.current_control = list()
# declare subscribers
self.trajectory_sub = rospy.Subscriber("/path", Path,
self.path_callback)
self.control_sub = rospy.Subscriber('/cmd_vel', Twist,
self.cmd_vel_callback)
# self.timer_ = rospy.Timer(rospy.Duration(0.033), self.timer_callback)
self.tf_sub = rospy.Subscriber('/tf', Twist, self.tf_callback)
if self.timeout > 0:
rospy.Timer(rospy.Duration(1), self.timeout_callback)
# set the function that will be executed when shutdown
rospy.on_shutdown(self.on_shutdown)
#!/usr/bin/env python
import math
import rospy
from clever import srv
from mavros_msgs.srv import CommandBool
from std_srvs.srv import Trigger
from geometry_msgs.msg import Point, PoseStamped, Quaternion, TransformStamped
from tf.transformations import quaternion_from_euler, euler_from_quaternion, quaternion_multiply
import tf2_ros
rospy.init_node('cow_despasturer_helper')
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
rate = rospy.Rate(10.0)
aruco_frame = 'aruco_0'
frame = 'map'
destination_pose = rospy.Publisher('~/destination_pose',
PoseStamped,
queue_size=40)
pose = PoseStamped()
pose.header.frame_id = "map"
trans = TransformStamped()
trans2 = TransformStamped()
trans3 = TransformStamped()
#trans.header.frame_id = "map"
broadcaster = tf2_ros.TransformBroadcaster()
static_bloadcaster = tf2_ros.StaticTransformBroadcaster()
while not rospy.is_shutdown():
def __init__(self, init_trans, update_freq):
rospy.Subscriber('aruco/markers', MarkerArray, self.update_callback)
rospy.Subscriber('sign_pose', MarkerArray, self.update_callback)
#rospy.Subscriber('marker_measurements', MarkerArray, self.update_callback)
rospy.Subscriber("cf1/pose", PoseStamped, self.cf1_pose_callback)
rospy.Subscriber("cf1/velocity", TwistStamped, self.cf1_vel_callback)
self.cf1_pose_cov_pub = rospy.Publisher("cf1/localizatiton/pose_cov",
PoseWithCovarianceStamped,
queue_size=1)
init_trans.header.frame_id = "map"
init_trans.child_frame_id = "cf1/odom"
self.init_trans = init_trans # remove?
self.last_transform = init_trans
self.cf1_pose = None
self.cf1_vel = None
self.old_msg = None
self.measurement_msg = None
# what translation/rotation variables to use in filtering
self.filter_config = rospy.get_param("localization/measurement_config")
self.tf_buf = tf2_ros.Buffer()
self.tf_lstn = tf2_ros.TransformListener(
self.tf_buf, queue_size=1) # should there be a queue_size?
self.broadcaster = tf2_ros.TransformBroadcaster()
self.static_broadcaster = tf2_ros.StaticTransformBroadcaster()
self.update_freq = update_freq
self.kf = KalmanFilter(
initial_cov=np.diag(rospy.get_param("localization/initial_cov")),
R=np.diag(rospy.get_param("localization/process_noise")),
maha_dist_thres=rospy.get_param("localization/maha_dist_thres"),
cov_norm_thres=rospy.get_param("localization/cov_norm_thres"),
delta_t=1.0 / self.update_freq)
self.maha_dist_thres = rospy.get_param("localization/maha_dist_thres")
self.cov_norm_thres = rospy.get_param("localization/cov_norm_thres")
self.converged_pub = rospy.Publisher("cf1/localization/converged",
Bool,
queue_size=1)
self.measurement_fb_pub = rospy.Publisher(
"cf1/localization/measurement_feedback",
Int32MultiArray,
queue_size=1)
self.odom_new_pub = rospy.Publisher("cf1/pose/odom_new",
PoseStamped,
queue_size=1)
self.believed_pub = rospy.Publisher("cf1/pose/believed",
PoseStamped,
queue_size=1)
self.measured_valid_pub = rospy.Publisher("cf1/pose/measured_valid",
PoseArray,
queue_size=1)
self.measured_invalid_pub = rospy.Publisher(
"cf1/pose/measured_invalid", PoseArray, queue_size=1)
self.filtered_pub = rospy.Publisher("cf1/pose/filtered",
PoseStamped,
queue_size=1)
rospy.Service("cf1/localization/reset_kalman_filter",
ResetKalmanFilter, self.reset_kalman_filter)
