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 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():
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 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():
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_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 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 getRobotLocation():
#track the robot
a = GetModelStateRequest(model_name='DD_robot')
s = boxProxy(a)
x = s.pose.position.x
y = s.pose.position.y
return [x, y]
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 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 __init__(self, load=False):
self.snn = Two_Layer_SNN(hidden_dim=30, load=load)
rospy.wait_for_service('/snake/publish_joint_commands')
self.commands_srv = rospy.ServiceProxy('/snake/publish_joint_commands',
PublishJointCmds)
rospy.wait_for_service('/gazebo/get_model_state')
self.get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
self.pos = []
self.model = GetModelStateRequest()
self.model.model_name = 'robot'
self.model.relative_entity_name = 'target::link'
self.model2 = GetModelStateRequest() # to get coordinate of snake head
self.model2.model_name = 'robot'
self.model2.relative_entity_name = ''
# self.measured_data = [0, 0]
self.control_cmd = [60 * np.pi / 180, 40 * np.pi / 180, 0, 0, 0.5]
self.dataset = {}
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 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 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()
def __init__(self, model_name, output_frame_id):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(LocateFactoryDeviceState, self).__init__(outcomes = ['succeeded', 'failed'], output_keys = ['pose'])
# Store state parameter for later use.
self._failed = True
# initialize service proxy
self._srv_name = '/gazebo/get_model_state'
self._srv = ProxyServiceCaller({self._srv_name: GetModelState})
self._srv_req = GetModelStateRequest()
self._srv_req.model_name = model_name # TODO: change parameter name
self._srv_req.relative_entity_name = output_frame_id # TODO: change parameter name
def __init__(self):
self.attServ = rospy.ServiceProxy('/link_attacher_node/attach', Attach)
self.detServ = rospy.ServiceProxy('/link_attacher_node/detach', Attach)
self.worldPropsServ = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
self.modelStateServ = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
self.linkStateServ = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState)
self.currentModel1 = 'robot'
self.currentModel2 = None
self.link1 = 'wrist_3_link'
self.link2 ='block'
# self.gripperMsg = AttachRequest()
# self.gripperMsg.link_name_1 = self.link1
# self.gripperMsg.link_name_2 = self.link2
# self.gripperMsg.model_name_1 = self.currentModel1
self.isGripping = False
self.gazeboModelsMsg = GetWorldPropertiesRequest()
self.gazeboModelStateMsg = GetModelStateRequest()
self.gazeboModelStateMsg.model_name = 'block_0'
self.gazeboLinkStateMsg = GetLinkStateRequest()
self.gazeboLinkStateMsg.link_name = 'wrist_3_link'
def __init__(self):
rospy.init_node('nav_node')
self.link_name_lst = [
'OPTIMUS::base_footprint', 'OPTIMUS::whFL', 'OPTIMUS::whFR',
'OPTIMUS::whRL', 'OPTIMUS::whRR'
]
self.joint_name_lst = ['j_whFR', 'j_whFL', 'j_whRR', 'j_whRL']
self.save_req_sub = rospy.Subscriber('/SaveDatPos', String,
self.save_req_callback)
self.nav_path_sub = rospy.Subscriber("/move_base/NavfnROS/plan", Path,
self.path_callback)
#Gazebo stuff
self.pause_proxy = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
self.unpause_proxy = rospy.ServiceProxy('/gazebo/unpause_physics',
Empty)
self.reset_world = rospy.ServiceProxy('/gazebo/reset_world', Empty)
self.get_model_state_proxy = rospy.ServiceProxy(
'/gazebo/get_model_state', GetModelState)
self.get_model_state_req = GetModelStateRequest()
self.get_model_state_req.model_name = 'OPTIMUS'
self.get_model_state_req.relative_entity_name = 'world'
self.get_link_state_proxy = rospy.ServiceProxy(
'/gazebo/get_link_state', GetLinkState)
self.get_link_state_req = GetLinkStateRequest()
self.get_link_state_req.link_name = '' # TODO: change
self.get_link_state_req.reference_frame = 'world'
self.filename = 'pos_file.csv'
# # make CSV file name from these params
# with open(self.filename, 'w') as f:
# f.write('name,x , y, z, roll, pitch, theta\n')
# self.flag = False
rospy.loginfo("[nav_node]: started")
self.navigate(self.load_nav_tasks(self.filename))
def __init__(self,
launch_file,
config,
port_ros='11311',
port_gazebo='11345',
reward_str='HitReward,CoinReward',
logger=None,
randomized_target=True,
action_space=((.3, .0), (.05, .3), (.05, -.3))):
print('port gazeebo: {}, port ros: {}'.format(port_gazebo, port_ros))
gazebo_env.GazeboEnv.__init__(self,
launch_file,
port=port_ros,
port_gazebo=port_gazebo)
print('ACTION SPACE', action_space)
self.vel_pub = rospy.Publisher('/mobile_base/commands/velocity',
Twist,
queue_size=5)
self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
self.pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
self.reset_proxy = rospy.ServiceProxy('/gazebo/reset_simulation',
Empty)
self.action_space = spaces.Discrete(len(action_space))
self.action_tuple = action_space
self.reward_range = (-np.inf, np.inf)
self.model_coordinates = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
self.model = GetModelStateRequest()
self.model.model_name = 'mobile_base'
self.path_planner = path_planners.PathPlanner(config,
randomized_target)
self.rewards = get_reward(reward_str,
config,
path_planner=self.path_planner)
self.min_range = 0.2
self.transform_observation = state_transforms.ObservationTransform(
config, self.path_planner)
self.logger = logger
self._seed()
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 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)