def get_angle_to_sphere(self):
"""
Using the positions of the robot and the sphere in gazebo, calculate the
angle between the two. Throws an error if the sphere or robot don't exist.
"""
#Get robot and sphere locations
sphere_model = GetModelStateRequest()
sphere_model.model_name = 'unit_sphere'
robot_model = GetModelStateRequest()
robot_model.model_name = 'turtlebot3_burger'
result_sphere = self.get_location_srv(sphere_model)
if not result_sphere.success:
rp.logerr(
'Error in getting sphere model locations - Have you added the unit_sphere to gazebo?'
)
return
result_robot = self.get_location_srv(robot_model)
if not result_sphere.success:
rp.logerr(
'Error in getting robot model locations - Have you added the unit_sphere to gazebo?'
)
return
sphere_x = result_sphere.pose.position.x
sphere_y = result_sphere.pose.position.y
robot_x = result_robot.pose.position.x
robot_y = result_robot.pose.position.y
#calculating the angle between the two
x_diff = sphere_x - robot_x
y_diff = sphere_y - robot_y
self.angle_to_sphere = math.atan2(y_diff, x_diff)
def record():
rospy.init_node('abp_recorder_dist')
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
m1 = GetModelStateRequest()
m2 = GetModelStateRequest()
m1.model_name = 'satlet0'
m2.model_name = 'satlet1'
f = open('logs/states.log', 'w')
global truth, estimate, update
rate = rospy.Rate(1)
counter = 0
while not rospy.is_shutdown():
req = get_model_srv(m1)
rp = req.pose.position
ra = req.pose.orientation
ang = euler_from_quaternion([ra.x, ra.y, ra.z, ra.w])
s1 = [rp.x, rp.y, ang[2]]
req = get_model_srv(m2)
rp = req.pose.position
ra = req.pose.orientation
ang = euler_from_quaternion([ra.x, ra.y, ra.z, ra.w])
s2 = [rp.x, rp.y, ang[2]]
f.write("%d,%s,t\n" % (counter, ",".join(map(str, s1))))
f.write("%d,%s,e\n" % (counter, ",".join(map(str, s2))))
counter += 1
rate.sleep()
f.close()
def teleport_entity(self, entity, pose):
"""
Teleports an model named by entity to give pose. Sometimes gazebo
teleportation fails, so a number of repeated attempts are made. An
error is printed if teleportation fails after repeated attempts.
THIS FUNCTION CANNOT BE CALLED WHILE GAZEBO IS PAUSED!! Any attempts
to check location while gazebo is paused will fail. Use carefully
"""
count = 0
attempts = 5
location_verified = False
while count < attempts and not location_verified:
get_state = GetModelStateRequest()
get_state.model_name = entity
resp = self.get_gazebo_model_state(get_state)
if check_close_poses(resp.pose, pose):
location_verified = True
else:
set_state = SetModelStateRequest()
set_state.model_state.model_name = entity
set_state.model_state.pose = pose
resp = self.set_gazebo_model_state(set_state)
if not resp.success:
rospy.logwarn("Failed attempt at moving object")
count = count + 1
if not location_verified:
rospy.logerr("Unable to move " + entity + " to " + str(pose)
+ " after " + attempts + " attempts.")
return location_verified
def main(model_name):
rospy.init_node('odom_pub')
print(model_name)
odom_pub = rospy.Publisher("/position_" + model_name,
Odometry,
queue_size=10)
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
odom = Odometry()
header = Header()
header.frame_id = '/odom'
model = GetModelStateRequest()
model.model_name = model_name
r = rospy.Rate(100)
while not rospy.is_shutdown():
result = get_model_srv(model)
odom.pose.pose = result.pose
odom.twist.twist = result.twist
header.stamp = rospy.Time.now()
odom.header = header
odom_pub.publish(odom)
print(model_name)
r.sleep()
def calc_torque(self):
mass = 0.3
px_arr = np.zeros(self.pile.num_particle)
py_arr = np.zeros(self.pile.num_particle)
pz_arr = np.zeros(self.pile.num_particle)
for i in range(1, self.pile.num_particle + 1):
if self.particle_index[i] == 1:
get_particle_state_req = GetModelStateRequest()
get_particle_state_req.model_name = 'particle' + str(i)
get_particle_state_req.relative_entity_name = 'bucket' # 'world'
particle_state = self.get_model_state_proxy(
get_particle_state_req)
x = particle_state.pose.position.x
y = particle_state.pose.position.y
z = particle_state.pose.position.z
orientation = particle_state.pose.orientation
(roll, pitch, theta) = euler_from_quaternion([
orientation.x, orientation.y, orientation.z, orientation.w
])
px_arr[i - 1] = x
py_arr[i - 1] = y
pz_arr[i - 1] = z
self.torque_sum += mass * 9.80665 * abs(x) * math.sin(roll)
def publish_ground_truth():
rospy.init_node('pose_ground_truth')
odom_pub = rospy.Publisher('/pose_ground_truth', Odometry, queue_size=10)
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
odom = Odometry()
header = Header()
header.frame_id = 'odom'
child_frame = 'base_link'
model = GetModelStateRequest()
model.model_name = 'rrbot'
r = rospy.Rate(20)
while not rospy.is_shutdown():
result = get_model_srv(model)
odom.pose.pose = result.pose
odom.twist.twist = result.twist
header.stamp = rospy.Time.now()
odom.header = header
odom.child_frame_id = child_frame
odom_pub.publish(odom)
r.sleep()
def get_model_state_twist(self, name):
req = GetModelStateRequest()
req.model_name = name
result = self._get_model_state(req)
if not result.success:
return False
return twist_to_list(result.twist)
def __init__(self):
rospy.init_node('change_pose', anonymous=True)
rospy.Subscriber("/velodyne_points", PointCloud2, self.callback_pointcloud)
get_state_service = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
model = GetModelStateRequest()
model.model_name = 'scout/'
def get_model_state(self, model_name):
req = GetModelStateRequest()
res = GetModelStateResponse()
req.model_name = model_name
try:
res = self.srv_get_model_state(req)
except rospy.service.ServiceException:
pass
return res
def get_model_pose(model_name, hover_flag):
rospy.wait_for_service('/gazebo/get_model_state')
req = GetModelStateRequest()
req.model_name = model_name
try:
client = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
res = client(req)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def main():
rospy.init_node('draw_GT_pose', anonymous=True)
rate = rospy.Rate(5)
get_state_service = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
model = GetModelStateRequest()
model.model_name = 'scout/'
pub = rospy.Publisher('/gt_trajectory_vis', Marker, queue_size=10)
vis_traj = Marker()
vis_traj.header.frame_id = "world" # gazebo输出的坐标是全局world坐标系上的
vis_traj.header.stamp = rospy.Time.now()
vis_traj.type = vis_traj.SPHERE_LIST
vis_traj.ns = "gt_traj"
vis_traj.action = vis_traj.ADD
vis_traj.id = 1
vis_traj.scale.x = 0.15
vis_traj.scale.y = 0.15
vis_traj.scale.z = 0.15
vis_traj.color.a = 0.5
vis_traj.color.r = 0.0
vis_traj.color.g = 1.0
vis_traj.color.b = 0.0
vis_traj.pose.orientation.x = 0.0
vis_traj.pose.orientation.y = 0.0
vis_traj.pose.orientation.z = 0.0
vis_traj.pose.orientation.w = 1.0
pt = Point()
pt_last = Point()
pt_last.x = 9999.0
pt_last.y = 9999.0
pt_last.z = 9999.0
while not rospy.is_shutdown():
objstate = get_state_service(model)
pt.x = objstate.pose.position.x
pt.y = objstate.pose.position.y
pt.z = objstate.pose.position.z
print("pt.x : %f, pt.y : %f, pt.z : %f, " % (pt.x, pt.y, pt.z))
print("pt_last.x : %f, pt_last.y : %f, pt_last.z : %f, " %
(pt_last.x, pt_last.y, pt_last.z))
print(point_dis(pt, pt_last))
print(point_dis(pt, pt_last) > 1.0)
if point_dis(pt, pt_last) > 1.0:
print("--------")
vis_traj.points.append(pt)
pub.publish(vis_traj)
pt_last = copy.deepcopy(pt)
rate.sleep()
def getRobotLocation():
a = GetModelStateRequest(model_name='cc_robot')
a.model_name = "cc_robot"
s = robot_proxy(a)
#print a
#print s
x = s.pose.position.x
y = s.pose.position.y
print "Robot Current Location:"
print "x = " + str(x) + " y = " + str(y)
return(x,y)
def get_robot_pose(self):
request = GetModelStateRequest()
request.model_name = self._model_name
request.relative_entity_name = self._world_link
response = self._get_model_state_service.call(request)
if not response.success:
rospy.logwarn(
'Problem when getting pose between %s and %s. Details: %s' %
(self._world_link, self._model_name, response.status_message))
return None
return response.pose
def getRobotLocation():
global robot_proxy
a = GetModelStateRequest(model_name='dd_robot')
a.model_name = "dd_robot"
s = robot_proxy(a)
#print a
#print s
x = s.pose.position.x
y = s.pose.position.y
print "x = " + str(x) + "y = " + str(y)
return (x, y)
def get_position_callback(self):
rospy.wait_for_service('/gazebo/get_model_state')
try:
get_state = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
get_position_msg = GetModelStateRequest()
get_position_msg.model_name = 'Ping Pong Ball'
state = get_state(get_position_msg)
self._ball_pos = (state.pose.position.x, state.pose.position.y,
state.pose.position.z)
except rospy.ServiceException, e:
print("Service call failed: %s" % e)
def getRobotLocation(): # Done!
global robotProxy
a = GetModelStateRequest(model_name="rob_0")
a.model_name = "rob_0"
s = robotProxy(a)
#print(a)
#print(s)
x = s.pose.position.x
y = s.pose.position.y
print(x, y)
print("x = ", str(x), ", y = ", str(y))
return (x, y)
def getRobotLocation():
global robot_proxy
a = GetModelStateRequest(model_name='dd_robot')
a.model_name = 'dd_robot'
s = robot_proxy(a)
#print(a)
#print(s)
x = s.pose.position.x
y = s.pose.position.y
return (x,y)
def get_location():
rospy.wait_for_service('/gazebo/get_model_state')
try:
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
model = GetModelStateRequest()
model.model_name = 'triton_lidar'
result = get_model_srv(model)
return result.pose.position
except rospy.ROSInterruptException:
rospy.loginfo("could not get model state")
def rosViewer():
service_list = rosservice.get_service_list()
print service_list
rospy.wait_for_service('/gazebo/get_model_state')
print "finished waiting for service..."
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
model = GetModelStateRequest()
model.model_name = 'piano2'
while not rospy.is_shutdown():
result = get_model_srv(model)
print '[', result.pose.position.x, ',', result.pose.position.y, ',', result.pose.position.z, ']'
time.sleep(1)
def main():
global recording
global this_check_move
global last_check_move
rospy.init_node('listener', anonymous=True)
rospy.wait_for_service('/gazebo/get_model_state')
print "Im starting the recording loop"
get_link_srv = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState)
link = GetLinkStateRequest()
link.link_name = 'wrist_3_link'
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
model = GetModelStateRequest()
model.model_name = 'robot'
while True:
rospy.Subscriber("writer", String, callback)
rospy.Subscriber("filestring", String, callback_file)
time.sleep(.1)
linkresult = get_link_srv(link) # we use for the link
modelresult = get_model_srv(
model) # robot will not move much in this example
x = linkresult.link_state.pose.position.x
this_check_move = x
we_moved = False
diff = abs(last_check_move - this_check_move)
if diff > 0.0001:
we_moved = True
y = linkresult.link_state.pose.position.y
z = linkresult.link_state.pose.position.z
print "recording", recording, "we moved", we_moved
if recording == True and we_moved == True:
print "We moved and are recording ", recording, "file ", file_to_write
try:
with open(file_to_write, 'a') as csvfile:
writer = csv.writer(csvfile,
delimiter=' ',
quotechar='|',
quoting=csv.QUOTE_MINIMAL)
writer.writerow([x, ",", y, ",", z])
except:
print "We may have a bad file ", file_to_write
last_check_move = this_check_move
def particle_location(self, num_p):
px_arr = np.zeros(num_p)
py_arr = np.zeros(num_p)
pz_arr = np.zeros(num_p)
for i in range(1, num_p + 1):
get_particle_state_req = GetModelStateRequest()
get_particle_state_req.model_name = 'particle' + str(i)
get_particle_state_req.relative_entity_name = 'base_footprint' # 'world'
particle_state = self.get_model_state_proxy(get_particle_state_req)
x = abs(particle_state.pose.position.x) + self.HALF_KOMODO
y = particle_state.pose.position.y
z = particle_state.pose.position.z
orientation = particle_state.pose.orientation
(roll, pitch, theta) = euler_from_quaternion(
[orientation.x, orientation.y, orientation.z, orientation.w])
px_arr[i - 1] = x
py_arr[i - 1] = y
pz_arr[i - 1] = z
return px_arr, pz_arr, py_arr
def odom_broadcast(self):
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
model = GetModelStateRequest()
model.model_name = "hyq"
result = get_model_srv(model)
odom = Odometry()
header = Header()
header.frame_id = 'base_link'
odom.pose.pose = result.pose
odom.twist.twist = result.twist
header.stamp = rospy.Time.now()
odom.header = header
br = tf.TransformBroadcaster()
br.sendTransform((odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z),
(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z,
odom.pose.pose.orientation.w),
rospy.Time.now(),
'base_link', 'map')
def main():
vehicle_ns = cfg.parse_args()
ns_obj = VehicleCfg(vehicle_ns)
node_name, topic_name, model_name, frame_id = ns_obj.get_odom_properties()
rospy.init_node(node_name, anonymous=True)
odom_pub = rospy.Publisher(topic_name, Odometry, queue_size=100)
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
header = Header()
odom = Odometry()
header.frame_id = frame_id
model = GetModelStateRequest()
model.model_name = model_name
rate = rospy.Rate(100)
while not rospy.is_shutdown():
result = get_model_srv(model)
odom.pose.pose = result.pose
odom.twist.twist = result.twist
header.stamp = rospy.Time.now()
odom.header = header
odom_pub.publish(odom)
rate.sleep()
from std_msgs.msg import Header
from gazebo_msgs.srv import GetModelState, GetModelStateRequest
rospy.init_node('odom_pub')
odom_pub = rospy.Publisher('/odom', Odometry)
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
odom = Odometry()
header = Header()
header.frame_id = '/odom'
model = GetModelStateRequest()
model.model_name = 'pinna_bot'
r = rospy.Rate(10)
while not rospy.is_shutdown():
result = get_model_srv(model)
odom.pose.pose = result.pose
odom.twist.twist = result.twist
header.stamp = rospy.Time.now()
odom.header = header
odom_pub.publish(odom)
r.sleep()
#!/usr/bin/env python3
import rospy
from nav_msgs.msg import Odometry
from std_msgs.msg import Header
from gazebo_msgs.srv import GetModelState, GetModelStateRequest
rospy.init_node('odom_d17')
odom_pub = rospy.Publisher("odom_d17", Odometry, queue_size=60)
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
odom = Odometry()
#header = Header()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
model = GetModelStateRequest()
model.model_name = 'drone17'
r = rospy.Rate(60)
while not rospy.is_shutdown():
result = get_model_srv(model)
odom.pose.pose = result.pose
odom.twist.twist = result.twist
odom.header.stamp = rospy.Time.now()
odom_pub.publish(odom)
r.sleep()
odom_pub = rospy.Publisher('/orange/odom', Odometry, queue_size=10)
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
odom = Odometry()
header = Header()
header.frame_id = '/orange/base_link'
model = GetModelStateRequest()
model.model_name = 'orange'
r = rospy.Rate(2)
while not rospy.is_shutdown():
result = get_model_srv(model)
odom.pose.pose = result.pose
odom.twist.twist = result.twist
header.stamp = rospy.Time.now()
odom.header = header
# if __name__ == '__main__':
# main()
#!/usr/bin/env python
import rospy
from gazebo_msgs.srv import GetModelStateRequest, GetModelState
from geometry_msgs.msg import Pose
from nav_msgs.msg import Odometry
import os, sys
root_path = sys.path[0]
f = open(os.path.join(root_path, 'ground_truth.txt'), 'w')
get_state_service = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
model = GetModelStateRequest()
model.model_name = 'scout/'
def callback(odometry):
time_stamp = odometry.header.stamp.to_sec()
objstate = get_state_service(model)
data = (time_stamp,
objstate.pose.position.x,
objstate.pose.position.y,
objstate.pose.position.z,
objstate.pose.orientation.x,
objstate.pose.orientation.y,
objstate.pose.orientation.z,
objstate.pose.orientation.w)
print("{}: ".format(data[0]))
print("{} {} {} {} {} {} {}".format(data[1], data[2], data[3], data[4], data[5], data[6], data[7]))
f.write("{} {} {} {} {} {} {} {}\n".format(data[0], data[1], data[2],
def get_model_state(self):
get_model_state_req = GetModelStateRequest()
get_model_state_req.model_name = self.camera_name
return self.get_model_state_service(get_model_state_req)
def does_world_contain_model(self, model_name="coke_can"):
get_model_state_req = GetModelStateRequest()
get_model_state_req.model_name = model_name
resp = self.get_model_state_service(get_model_state_req)
return resp.success
def get_model_state(self,model_name="coke_can"):
get_model_state_req = GetModelStateRequest()
get_model_state_req.model_name = model_name
return self.get_model_state_service(get_model_state_req)
def controller():
global state, desired, lock_yaw
name = rospy.get_namespace()
rospy.init_node(name[1:-1] + '_controller')
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
rospy.Service(name + 'desired', DesiredCoords, handle_desired)
model = GetModelStateRequest()
model.model_name = name[1:-1]
cmd_pub = rospy.Publisher(name + 'velocity',
Float32MultiArray,
queue_size=4)
cmd = Float32MultiArray()
desired[0] = float(sys.argv[1])
desired[1] = float(sys.argv[2])
desired[2] = float(sys.argv[3])
rate = rospy.Rate(20)
while not rospy.is_shutdown():
req = get_model_srv(model)
rp = req.pose.position
ra = req.pose.orientation
ang = euler_from_quaternion([ra.x, ra.y, ra.z, ra.w])
state = [rp.x, rp.y, ang[2]]
if lock_yaw:
vel = get_velocity(desired, state)
alt = []
alt.append(4 * vel[0] / abs(vel[0]) if vel[0] else 0.)
alt.append(4 * vel[1] / abs(vel[1]) if vel[1] else 0.)
alt.append(3 * vel[2] / abs(vel[2]) if vel[2] else 0.)
vel[0] = min(vel[0], alt[0], key=abs)
vel[1] = min(vel[1], alt[1], key=abs)
vel[2] = min(vel[2], alt[2], key=abs)
cmd.data = vel
#print(desired)
else:
dist = distance(desired, state)
if dist == 0.:
dira = state[2]
else:
dira = cos((desired[0] - state[0]) / dist)
if dist > 0.25 and abs(dira - state[2]) > 0.1:
vel = get_velocity([0., 0., dira], state)
cmd.data = [0., 0., vel[2]]
elif dist > 0.25:
vel = get_velocity(desired, state)
alt = []
alt.append(4 * vel[0] / abs(vel[0]) if vel[0] else 0.)
alt.append(4 * vel[1] / abs(vel[1]) if vel[1] else 0.)
alt.append(3 * vel[2] / abs(vel[2]) if vel[2] else 0.)
vel[0] = min(vel[0], alt[0], key=abs)
vel[1] = min(vel[1], alt[1], key=abs)
#vel[2] = min(vel[2], alt[2], key=abs)
vel[2] = 0.
cmd.data = vel
elif dist < 0.25 and abs(desired[2] - state[2]) > 0.1:
vel = get_velocity(desired, state)
cmd.data = [0., 0., vel[2]]
#if name[1:-1] is 'satlet0':
# print(cmd.data)
cmd_pub.publish(cmd)
rate.sleep()
#!/usr/bin/env python3
import rospy
from nav_msgs.msg import Odometry
from std_msgs.msg import Header
from gazebo_msgs.srv import GetModelState, GetModelStateRequest
rospy.init_node('odom_d7')
odom_pub = rospy.Publisher("odom_d7", Odometry, queue_size=60)
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
odom = Odometry()
#header = Header()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
model = GetModelStateRequest()
model.model_name = 'drone07'
r = rospy.Rate(60)
while not rospy.is_shutdown():
result = get_model_srv(model)
odom.pose.pose = result.pose
odom.twist.twist = result.twist
odom.header.stamp = rospy.Time.now()
odom_pub.publish(odom)
r.sleep()
from std_msgs.msg import Header
from gazebo_msgs.srv import GetModelState, GetModelStateRequest
rospy.init_node('odom_pub')
odom_pub = rospy.Publisher('/my_odom', Odometry, queue_size=5)
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
odom = Odometry()
header = Header()
header.frame_id = '/odom'
model = GetModelStateRequest()
model.model_name = 'mir'
r = rospy.Rate(2) #how often odometry publihes
while not rospy.is_shotdown():
result = get_model_srv(model)
odom.pose.pose = result.pose
odom.twist.twist = result.twist
header.stamp = rospy.Time.now()
odom.header = header
odom_pub.publish(odom)
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
br = tf.TransformBroadcaster()
odom = Odometry()
header = Header()
header.frame_id = '/odom'
datapreprocessing_pose = Pose2D()
datapreprocessing_velocity = Float32()
model = GetModelStateRequest()
model.model_name = 'cc_2019_car'
r = rospy.Rate(2)
while not rospy.is_shutdown():
result = get_model_srv(model)
odom.pose.pose = result.pose
odom.twist.twist = result.twist
header.stamp = rospy.Time.now()
odom.header = header
odom_pub.publish(odom)
quaternion = (result.pose.orientation.x, result.pose.orientation.y,
result.pose.orientation.z, result.pose.orientation.w)