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)