def visualize():
global points_flat
global leg
if points_flat == None:
points_flat = []
for l in points:
for n in l:
points_flat.append(n)
if path_pub.get_num_connections() > 0:
frame_id = 'map'
pa = Path()
pa.header = Utils.make_header(frame_id)
pa.poses = []
for n in points_flat:
n2 = [
n[0] * PIXELS_TO_METERS + map_x,
n[1] * PIXELS_TO_METERS + map_y, 0
]
pa.poses.append(Utils.particle_to_posestamped(n2, str(frame_id)))
path_pub.publish(pa)
if leg_pub.get_num_connections() > 0:
frame_id = 'map'
pa = Path()
pa.header = Utils.make_header(frame_id)
pa.poses = []
for n in points[leg]:
n2 = [
n[0] * PIXELS_TO_METERS + map_x,
n[1] * PIXELS_TO_METERS + map_y, 0
]
pa.poses.append(Utils.particle_to_posestamped(n2, str(frame_id)))
leg_pub.publish(pa)
def publisher(paths):
publisher = rospy.Publisher('pathfinder', Path, queue_size=20)
rospy.init_node('path_publisher', anonymous=True)
rate = rospy.Rate(10) # 10 hertz
msgs=[] #list for all the messages we need to publish
for path in paths:
msg = Path() #new path message
msg.header = create_std_h() #add a header to the path message
for x,y,z in path:
#create a pose for each position in the path
#position is the only field we care about, leave the rest as 0
ps = PoseStamped()
ps.header=create_std_h()
i=Pose()
i.position=Point(x,y,z)
ps.pose=i
msg.poses.append(ps)
msgs.append(msg)
while not rospy.is_shutdown(): # while we're going
#publish each message
for m in msgs:
#rospy.loginfo(m)
publisher.publish(m)
rate.sleep()
def cb(data):
print("RECEIVED GEO PATH MESSAGE")
global pub
global lastSatPos
global listener
print(data)
if lastSatPos is None:
print("NO SAT DATA, FAIL")
return
print(lastSatPos)
path = Path()
path.header = data.header
path.header.frame_id = "map" # not sure why this isnt published :3
poses = []
ps = PoseStamped()
ps.header = data.header
ps.pose.orientation = data.pose.orientation
transformCoordinates(data.pose.position, ps.pose.position)
# move into map frame
ps.header.frame_id = 'map'
poses.append(ps)
path.poses = poses
pub.publish(path)
print(path)
def collision_avoidance_client(path_local):
global path
# Waits until the action server has started up and started
# listening for goals.
print("Waiting for server")
client.wait_for_server()
print("Connected to server")
# Creates a goal to send to the action server.
print("Create goal")
goal = collision_avoidance.msg.PathControlGoal(path=path_local)
path = Path()
path.header = path_local.header
path.header.stamp = rospy.Time.now()
pub.publish(path)
# Sends the goal to the action server.
print("Send goal to server")
client.send_goal(goal)
# Waits for the server to finish performing the action.
print("Waiting for result")
client.wait_for_result()
# Prints out the result of executing the action
print("Done")
def talker():
rospy.init_node('path_publisher')
pub_path = rospy.Publisher('path', Path)
pub_pose = rospy.Publisher('pose', PoseStamped)
path = Path()
rospy.loginfo(tf.transformations.quaternion_from_euler(0, 0, 0))
path.header = Header(frame_id='workobject')
#path.header = Header(frame_id='tool0')
#path.poses = [PoseStamped(pose=Pose(Point(0, 0, 0.5), Quaternion(0, 0, 0, 1))),
#PoseStamped(pose=Pose(Point(0, 1, 1.4), Quaternion(0, 0, 0, 1)))]
layers = dxf2path.read_layers(rospkg.RosPack().get_path('etna_planning') + '/src/robpath/models_dxf/curvas_v16.dxf')
points1, frames1 = dxf2path.get_vectors(layers['Copa1_I'], layers['Copa1_E'])
cut_path = dxf2path.frames2path(points1, frames1)
for cut_pose in cut_path:
(x, y, z), (q0, q1, q2, q3), proc = cut_pose
path.poses.append(PoseStamped(pose=Pose(Point(x/1000, y/1000, z/1000), Quaternion(q0, q1, q2, q3))))
pub_path.publish(path)
rospy.sleep(2.0)
k = 0
N = len(cut_path)
while not rospy.is_shutdown() and (k < N):
(x, y, z), (q0, q1, q2, q3), proc = cut_path[k]
rospy.loginfo("%s, %s" %(cut_path[k], rospy.get_time()))
pose = PoseStamped(Header(frame_id='workobject'),
Pose(Point(x/1000, y/1000, z/1000),
Quaternion(q0, q1, q2, q3)))
pub_pose.publish(pose)
k = k + 1
rospy.sleep(1.0)
def on_enter(self, userdata): path = Path() path.header = userdata.path.header for p in userdata.path.poses: # always add first pose if len(path.poses) == 0: path.poses.append(p) path.header.stamp = rospy.Time.now() + rospy.Duration(.5) continue last_pose = path.poses[-1] p.header.stamp = last_pose.header.stamp + rospy.Duration(1) # check distance l_xyz = [last_pose.pose.position.x, last_pose.pose.position.y, last_pose.pose.position.z] xyz = [p.pose.position.x, p.pose.position.y, p.pose.position.z] dist = math.sqrt(sum((p-l)*(p-l) for l,p in zip(l_xyz, xyz))) if dist >= self._max_dist: path.poses.append(p) continue if dist < self._min_dist: continue # check orientation diff l_q = last_pose.pose.orientation l_rpy = tf.transformations.euler_from_quaternion([l_q.x, l_q.y, l_q.z, l_q.w]) q = p.pose.orientation rpy = tf.transformations.euler_from_quaternion([q.x, q.y, q.z, q.w]) if abs(rpy[2] - l_rpy[2]) >= self._max_angle: path.poses.append(p) continue userdata.path = path
def get_plan(data):
start = PoseStamped()
start.header.frame_id = 'map'
rospy.wait_for_message('elektron/mobile_base_controller/odom', Odometry)
start.pose.position.x = current_pos[0]
start.pose.position.y = current_pos[1]
goal = PoseStamped()
goal.header.frame_id = 'map'
goal.pose.position.x = data[0]
goal.pose.position.y = data[1]
make_path_srv = rospy.ServiceProxy('/global_planner/planner/make_plan',
GetPlan)
path = make_path_srv(start=start, goal=goal, tolerance=0.0).plan
new_path = Path()
new_path.header = path.header
new_path.poses = [
path.poses[i] for i in range(
len(path.poses) / 10, len(path.poses),
len(path.poses) / 10)
]
new_path.poses.append(path.poses[-1])
path_follower_srv = rospy.ServiceProxy('path_follower', PathFollower)
resp1 = path_follower_srv(new_path)
def interpolate_path(self, path, distance):
'''
path is a ROS path message, interpolate the path so that the waypoits distance
are *distance*
return ros path message
'''
x_list = []
y_list = []
dist_list = []
for pose_stamped in path.poses:
x_list.append(pose_stamped.pose.position.x)
y_list.append(pose_stamped.pose.position.y)
if len(x_list) == 1:
dist_list.append(0)
else:
dist_incrment = math.sqrt((x_list[-1] - x_list[-2])**2 +
(y_list[-1] - y_list[-2])**2)
dist_list.append(dist_incrment + dist_list[-1])
dist_interp = [distance * i for i in range(self.steps)]
x_interp = np.interp(dist_interp, dist_list, x_list)
y_interp = np.interp(dist_interp, dist_list, y_list)
interp_traj = Path()
interp_traj.header = path.header
for k in range(len(x_interp)):
pose_stamped = PoseStamped()
pose_stamped.header = path.header
pose_stamped.pose.position.x = x_interp[k]
pose_stamped.pose.position.y = y_interp[k]
interp_traj.poses.append(pose_stamped)
return interp_traj
def path_reader(directory):
global subscribed_path
posestamp_list = []
seq = 0
header_msg = Header()
header_msg.seq = seq
header_msg.stamp = rospy.Time.now()
header_msg.frame_id = global_frame_id
loaded_path = np.load(directory).get('path')
for point in loaded_path:
path_msg = Path()
posestamp_msg = PoseStamped()
pose_msg = Pose()
pose_msg.position.x = -point[1]
pose_msg.position.y = point[0]
pose_msg.position.z = 0
posestamp_msg.pose = pose_msg
posestamp_msg.header = header_msg
posestamp_list.append(posestamp_msg)
header_msg.seq = seq
header_msg.stamp = rospy.Time.now()
header_msg.frame_id = global_frame_id
path_msg.header = header_msg
path_msg.poses = posestamp_list
path_pub.publish(path_msg)
seq += 1
subscribed_path = path_msg
def visualize(self, path, start, goal):
'''
Publish various visualization messages.
'''
#rospy.loginfo('visualizing start')
s = PointStamped()
s.header = Utils.make_header("/map")
s.point.x = start[0]
s.point.y = start[1]
s.point.z = 0
self.start_pub.publish(s)
#rospy.loginfo('visualizing goal')
g = PointStamped()
g.header = Utils.make_header("/map")
g.point.x = goal[0]
g.point.y = goal[1]
g.point.z = 0
self.goal_pub.publish(g)
#rospy.loginfo('visualizing path')
p = Path()
p.header = Utils.make_header("/map")
for point in path:
pose = PoseStamped()
pose.header = Utils.make_header("/map")
pose.pose.position.x = point[0]
pose.pose.position.y = point[1]
pose.pose.orientation = Utils.angle_to_quaternion(0)
p.poses.append(pose)
self.path_pub.publish(p)
def odom_cb(data):
path = Path()
path.header = data.header
pose = PoseStamped()
pose.header = data.header
pose.pose = data.pose.pose
path.poses.append(pose)
path_pub.publish(path)
def visualize_plan(self, grand_plan):
# grand_plan [array, array, ..]
path = Path()
path.header = Utils.make_header('map')
# plan: N x 3
# grand_plan: [ Nx3, Mx3, Kx3, ...] -> (N+M+K)x3
path.poses = map(Utils.config_to_posestamped, np.vstack(grand_plan))
self.viz_plan_pub.publish(path)
def visualize(self, poses):
if self.path_pub.get_num_connections() > 0:
frame_id = 'map'
for i in range(0, self.num_viz_paths):
pa = Path()
pa.header = Utils.make_header(frame_id)
pa.poses = map(Utils.particle_to_posestamped, poses[i,:,:], [frame_id]*self.T)
self.path_pub.publish(pa)
def path_publisher(robot_name):
while not rospy.is_shutdown():
try:
robotPath = rospy.get_param(robot_name + '/path')
robotPath = literal_eval(robotPath)
if len(robotPath) == 0:
continue
pub = rospy.Publisher(robot_name + '/path', Path, queue_size=10)
i = 1
path = Path()
for tup in robotPath:
newPoint = Point()
newPoint.x = tup[0]
newPoint.y = tup[1]
newPoint.z = 0
newRot = Quaternion()
newRot.x = 0
newRot.y = 0
newRot.z = 0
newRot.w = 1
newPose = Pose()
newPose.position = newPoint
newPose.orientation = newRot
newPoseStamped = PoseStamped()
newHeader = Header()
newHeader.seq = i
Header()
i+=1
newHeader.stamp = rospy.Time()
newHeader.frame_id = "world"
newPoseStamped.pose = newPose
newPoseStamped.header = newHeader
path.poses.append(newPoseStamped)
newPathHeader = Header()
newPathHeader.seq = i
newPathHeader.stamp = rospy.Time()
newPathHeader.frame_id = "world"
'''
path.color.a = colorLibrary[robot_name][0]# Don't forget to set the alpha!
path.color.r = colorLibrary[robot_name][1]
path.color.g = colorLibrary[robot_name][2]
path.color.b = colorLibrary[robot_name][3]
'''
path.header = newPathHeader
pub.publish(path)
sleep(5)
except Exception as e:
print(e)
def odom_callback(data):
global path
pose = PoseStamped()
pose.header = data.header
pose.pose = data.pose.pose
path.append(pose)
path_to_pub = Path()
path_to_pub.header = data.header
for p in path:
path_to_pub.poses.append(p)
path_pub.publish(path_to_pub)
def talker():
rospy.init_node('path_publisher', anonymous=True)
pub = rospy.Publisher('/path', Path, queue_size=1)
package_path = rospack.get_path('ackermann_vehicle_navigation')
test_directory = package_path + "/path/test_path.txt"
path_directory = rospy.get_param('~tracking_path_directory',
test_directory)
global_frame_id = rospy.get_param('~global_frame_id', 'world')
f = open(path_directory, 'r')
rospy.loginfo(path_directory)
rate = rospy.Rate(1) # 1hz
lines = f.readlines()
f.close()
posestamp_list = []
seq = 0
header_msg = Header()
header_msg.seq = seq
header_msg.stamp = rospy.Time.now()
header_msg.frame_id = global_frame_id
for line in lines:
path_msg = Path()
posestamp_msg = PoseStamped()
pose_msg = Pose()
value = line.split()
# rospy.loginfo(value)
pose_msg.position.x = float(value[0])
pose_msg.position.y = float(value[1])
pose_msg.position.z = 0
# pose_msg.orientation.x = 0
# pose_msg.orientation.y = 0
# pose_msg.orientation.z = float(value[5])
# pose_msg.orientation.w = float(value[6])
posestamp_msg.pose = pose_msg
posestamp_msg.header = header_msg
posestamp_list.append(posestamp_msg)
# rospy.loginfo(posestamp_list)
while not rospy.is_shutdown():
header_msg.seq = seq
header_msg.stamp = rospy.Time.now()
header_msg.frame_id = global_frame_id
path_msg.header = header_msg
path_msg.poses = posestamp_list
pub.publish(path_msg)
seq += 1
rate.sleep()
def visualize(self, poses):
if self.path_pub.get_num_connections() > 0:
frame_id = 'map'
for i in range(0, self.num_viz_paths):
pa = Path()
pa.header = Utils.make_header(frame_id)
pa.poses = []
for pose in poses.cpu().numpy()[i, :, :]:
pa.poses.append(
Utils.particle_to_posestamped(pose, str(frame_id)))
self.path_pub.publish(pa)
def path_message(PATH):
path = Path()
path.header = Header(frame_id="map", stamp=rospy.Time.now())
for pt in PATH:
pose = PoseStamped()
pose.header = path.header
pose.pose.position = Point(pt[0], pt[1], 10)
pose.pose.orientation = Quaternion(0, 0, 0, 1)
path.poses.append(pose)
return path
def visualize(self, poses):
if self.path_pub.get_num_connections() > 0:
frame_id = 'map'
for i in range(0, self.num_viz_paths):
pa = Path()
pa.header = Utils.make_header(frame_id)
particle_trajectory = poses[:,i,:] # indexed [pose, particle, time]
particle_trajectory = torch.from_numpy(particle_trajectory.cpu().numpy().transpose())
if False and i == 0:
print("trajectory", i, ": ", particle_trajectory)
pa.poses = map(Utils.particle_to_posestamped, particle_trajectory, [frame_id]*self.T)
self.path_pub.publish(pa)
def publish_path(self, waypoints, publisher):
# Visualize path derived from the given waypoints in the path
path = Path()
path.header = self.create_header('map')
path.poses = []
for point in waypoints:
tempPose = PoseStamped()
tempPose.header = path.header
tempPose.pose.position.x = point[0]
tempPose.pose.position.y = point[1]
tempPose.pose.orientation.w = 1.0
path.poses.append(tempPose)
publisher.publish(path)
def run(self):
while not rospy.is_shutdown():
# Publish path one at a time for allowing good
# rviz visualization. Set Buffer on rviz.
for id in self.trajectories.keys():
trajs = self.trajectories[id]
if len(trajs) != 0:
path = Path()
path.header = trajs[0].header
path.header.stamp = rospy.get_rostime()
path.poses = trajs
self.path_pub.publish(path)
def callback(data):
mypath = Path()
mypath.header = data.header
posestamped = PoseStamped()
posestamped.header = data.header
posestamped.pose.position = data.pose.postion
posestamped.pose.orientation = data.pose.orientation
mypath.pose.position = posestamped.pose.position
mypath.pose.orientation = posestamped.pose.orientation
pub = rospy.Publisher('/append_plot', plot, queue_size=10)
pub.publish(position)
def callback(self, data):
cv_img = self.bridge.imgmsg_to_cv2(data, "rgb8")
# Run pipeline
mask_img = self.Detector.filter(cv_img)
blur_img = self.Detector.blur_mask(mask_img)
warped_image = self.Detector.perspective_warp(blur_img)
try:
(left, center, right) = self.Detector.sliding_window(warped_image)
waypoints = self.Detector.generate_waypoints(cv_img, center)
# Generate publishing stuff
lane_image = self.Detector.draw_lanes(cv_img, left, right)
path = Path()
path.header = data.header
num_points = waypoints.shape[1]
for i in range(num_points):
x = float(waypoints[0, i])
y = float(waypoints[1, i])
theta = waypoints[2, i]
w = np.cos(theta / 2)
z = np.sin(theta / 2)
pose = PoseStamped()
p = Pose()
p.position.x = x
p.position.y = y
p.position.z = 0
p.orientation.x = 0.0
p.orientation.y = 0.0
p.orientation.z = z
p.orientation.w = w
pose.pose = p
"""
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientaion.z = 0
pose.pose.orientation.w = 1
"""
pose.header = data.header
path.poses.append(pose)
self.nav_pub.publish(path)
self.visualization_pub.publish(
self.bridge.cv2_to_imgmsg(lane_image, 'rgb8'))
except:
print("Failed to generate path")
rospy.logerr("LOLNO")
# Publish messages
self.mask_pub.publish(self.bridge.cv2_to_imgmsg(warped_image, 'mono8'))
def pc_callback(self, points):
if points.width < self.min_points:
rospy.logwarn('Selection too small, select more points.')
return
else:
header = points.header
xyz = np.copy(ros_numpy.numpify(points))
rospy.logdebug('Processing pointcloud...')
pcd = o3d.geometry.PointCloud()
xyz = np.array(xyz.tolist())
pcd.points = o3d.utility.Vector3dVector(xyz)
rospy.logdebug('Removing outliers...')
cl, ind = o3d.geometry.radius_outlier_removal(pcd,
nb_points=16,
radius=0.08)
pcd_clean = o3d.geometry.select_down_sample(cl, ind)
# print('Estimating normals...')
# o3d.geometry.estimate_normals(pcd_clean,search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1,max_nn=30))
#o3d.geometry.orient_normals_to_align_with_direction(pcd_clean,orientation_reference=[0.,0., -1.])
# o3d.geometry.orient_normals_towards_camera_location(pcd_clean)
rospy.logdebug('Downsampling points...')
downpcd = o3d.geometry.voxel_down_sample(
pcd_clean, voxel_size=self.sample_density)
points = np.asarray(downpcd.points)
norms = np.asarray(downpcd.normals)
rospy.logdebug("Points to plan through: ", points.shape[0])
rospy.logdebug('Travelling salesman...')
route = self.travelling_salesman(points.copy())
rospy.logdebug('Publishing path')
path = Path()
path.header = header
for r in route:
pose = PoseStamped()
pose.pose.orientation = self.get_orientation().orientation
pose.header = header
pose.pose.position.x = points[r, 0]
pose.pose.position.y = points[r, 1]
pose.pose.position.z = points[r, 2]
path.poses.append(pose)
self.traj_pub.publish(path)
def path_from_coordinates(waypoints):
path = Path()
path.header = Header()
path.header.frame_id = "map"
path.poses = []
count = 0
# convert x,y coordinates to PoseStamped
for waypoint in waypoints:
pose = PoseStamped()
pose.header = Header()
pose.header.seq = count
pose.header.frame_id = "map"
pose.pose.position.x = waypoint[0]
pose.pose.position.y = waypoint[1]
pose.pose.position.z = 0
path.poses.append(pose)
count += 1
# update the orientation of the goals
for i in range(len(path.poses) - 1):
pose = path.poses[i]
next_pose = path.poses[i+1]
# print(pose.pose.position)
# print(next_pose.pose.position)
# print("-----------------------")
d_x = next_pose.pose.position.x - pose.pose.position.x
d_y = next_pose.pose.position.y - pose.pose.position.y
yaw = math.atan2(d_y, d_x)
orientation = convert_to_quaternion(0, 0, yaw)
path.poses[i].pose.orientation.x = orientation[0]
path.poses[i].pose.orientation.y = orientation[1]
path.poses[i].pose.orientation.z = orientation[2]
path.poses[i].pose.orientation.w = orientation[3]
orientation = convert_to_quaternion(math.pi / 2, 0, 0)
path.poses[-1].pose.orientation.x = orientation[0]
path.poses[-1].pose.orientation.y = orientation[1]
path.poses[-1].pose.orientation.z = orientation[2]
path.poses[-1].pose.orientation.w = orientation[3]
return path
def talker():
f = open("path_final.txt", 'r')
f_ = open("path.txt", 'r')
lines = f.readlines()
lines_ = f_.readlines()
f.close()
f_.close()
posestamp_list = []
posestamp_list_ = []
seq = 0
seq_ = 0
header_msg = Header()
header_msg.seq = seq
header_msg.stamp = rospy.Time.now()
header_msg.frame_id = "map"
header_msg_ = Header()
header_msg_.seq = seq_
header_msg_.stamp = rospy.Time.now()
header_msg_.frame_id = "map"
for line in lines:
path_msg = Path()
posestamp_msg = PoseStamped()
pose_msg = Pose()
value = line.split()
pose_msg.position.x = float(value[0])
pose_msg.position.y = float(value[1])
pose_msg.position.z = float(0.0)
quaternion = (float(value[3]), float(value[4]), float(value[5]),
float(value[6]))
euler = tf.transformations.euler_from_quaternion(quaternion)
quaternion_b = tf.transformations.quaternion_from_euler(0, 0, euler[2])
pose_msg.orientation.x = float(quaternion_b[0])
pose_msg.orientation.y = float(quaternion_b[1])
pose_msg.orientation.z = float(quaternion_b[2])
pose_msg.orientation.w = float(quaternion_b[3])
posestamp_msg.pose = pose_msg
posestamp_msg.header = header_msg
posestamp_list.append(posestamp_msg)
header_msg.seq = seq
header_msg.stamp = rospy.Time.now()
header_msg.frame_id = "map"
path_msg.header = header_msg
path_msg.poses = posestamp_list
return path_msg
def publish_steps(self, steps):
path = Path()
for step in steps:
pose = PoseStamped()
pose.header = Header()
pose.header.frame_id = strings.odom_frame
pose.header.stamp = rospy.Time.now()
pose.pose = utils.pose2D_to_pose(step.pose)
path.poses.append(pose)
path.header = Header()
path.header.frame_id = strings.odom_frame
path.header.stamp = rospy.Time.now()
self._pathPubl.publish(path)
return
def PlanPath(self, goal_config):
"""Processes service request (query for path to goal point).
Path is published in nav_msgs/Path type.
"""
print("Starting PRM PlanPath.")
start_config = self.GetRobotPose()
prm_path = self.prm.FindPath(start_config, goal_config)
# Dummy path
# corner1 = np.array([0.2, 0, -np.pi/2])
# corner2 = np.array([0.2, -0.2, np.pi])
# corner3 = np.array([0, -0.2, np.pi/2])
# prm_path = [corner1, corner2, corner3 , goal_config]
# Make sure start_config is not in path!
# Process path and publish as nav_msgs/Path
# - header
# - geometry_msgs/PoseStamped[] poses
if len(prm_path) != 0:
plan_msg = Path()
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
plan_msg.header = h
pub_path = []
print("Path: ")
for config in prm_path[
1:]: #ignore first node in path; it's the start config
pose = PoseStamped()
pose.pose.position.x, pose.pose.position.y = config[0], config[
1]
quat = tf.transformations.quaternion_from_euler(
0, 0, config[2])
pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quat[
0], quat[1], quat[2], quat[3]
pub_path.append(pose)
print(pose.pose.position.x, pose.pose.position.y, config[2])
plan_msg.poses = pub_path
self.plan_pub.publish(plan_msg)
print("Published path to prm_path.")
return True
def talker():
pub = rospy.Publisher('/path', Path, queue_size=10)
rospy.init_node('path_publisher', anonymous=True)
rate = rospy.Rate(10) # 10hz
f = open("path.txt", 'r')
lines = f.readlines()
f.close()
posestamp_list = []
seq = 0
header_msg = Header()
header_msg.seq = seq
header_msg.stamp = rospy.Time.now()
header_msg.frame_id = "map"
for line in lines:
path_msg = Path()
posestamp_msg = PoseStamped()
pose_msg = Pose()
value = line.split()
#rospy.loginfo(value)
pose_msg.position.x = float(value[0])
pose_msg.position.y = float(value[1])
pose_msg.position.z = float(value[2])
quaternion = (float(value[3]), float(value[4]), float(value[5]),
float(value[6]))
euler = tf.transformations.euler_from_quaternion(quaternion)
quaternion_b = tf.transformations.quaternion_from_euler(0, 0, euler[2])
pose_msg.orientation.x = float(quaternion_b[0])
pose_msg.orientation.y = float(quaternion_b[1])
pose_msg.orientation.z = float(quaternion_b[2])
pose_msg.orientation.w = float(quaternion_b[3])
posestamp_msg.pose = pose_msg
posestamp_msg.header = header_msg
posestamp_list.append(posestamp_msg)
rospy.loginfo(posestamp_list)
while not rospy.is_shutdown():
header_msg.seq = seq
header_msg.stamp = rospy.Time.now()
header_msg.frame_id = "map"
path_msg.header = header_msg
path_msg.poses = posestamp_list
pub.publish(path_msg)
seq += 1
rate.sleep()
def traj2path(trajectory):
path = Path()
h = Header()
path.header = h
h.frame_id = 'map'
h.stamp = rospy.Time.now()
for pt in trajectory:
p = PoseStamped()
p.header = h
p.pose = Pose()
p.pose.position = Point(pt[0], pt[1], 0)
if len(pt) == 3:
q = tf.transformations.quaternion_from_euler(0, 0, pt[2])
p.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
path.poses.append(p)
return path
def callback(self, msg):
out = Path()
in_pose = msg.pose
pose_stamped = PoseStamped
pose_stamped.pose = in_pose
pose_stamped.header = msg.header
pose_stamped.header.frame_id = 'base'
out.header = msg.header
out.header.frame_id = 'base'
#out.header = msg.header
self.poses.append(pose_stamped)
out.poses = self.poses
self.pub.publish(out)
return()
def read_waypoints_from_csv(self, filename):
'''read waypoints from given csv file and return the data in the form of nav_msgs::Path'''
path = Path()
path.header = self.create_header('map')
if filename == '':
raise ValueError('No any file path for waypoints file')
with open(filename) as f:
path_points = [tuple(line) for line in csv.reader(f, delimiter=',')]
path_points = [(float(point[0]), float(point[1]), float(point[2])) for point in path_points]
skip=6
for idx,point in enumerate(path_points):
if idx%skip ==0:
header = self.create_header('map')
waypoint = Pose(Point(float(point[0]), float(point[1]), 0), self.heading(0.0))
path.poses.append(PoseStamped(header, waypoint))
return path
def talker():
pub = rospy.Publisher('path', Path)
pub_pose = rospy.Publisher('pose', PoseStamped)
pub_traj = rospy.Publisher('joint_path_command', JointTrajectory)
rospy.init_node('path_publisher')
path = Path()
rospy.loginfo(tf.transformations.quaternion_from_euler(0, 0, 0))
#path.header = Header(frame_id='work_object')
path.header = Header(frame_id=FRAME)
#path.poses = [PoseStamped(pose=Pose(Point(0, 0, 0.5), Quaternion(0, 0, 0, 1))),
#PoseStamped(pose=Pose(Point(0, 1, 1.4), Quaternion(0, 0, 0, 1)))]
traj = JointTrajectory()
traj.points = [
JointTrajectoryPoint(positions=[1.0, 0.0, 0.0, 0.0, 0.0, 0.0])
]
rospy.loginfo(traj)
pub_traj.publish(traj)
layers = dxf.read_layers(
'/home/jorge/ros_workspace/robot_path/curvas_v16.dxf')
points1, frames1 = dxf2path.get_vectors(layers['Copa1_I'],
layers['Copa1_E'])
cut_path = dxf2path.frames2path(points1, frames1)
for cut_pose in cut_path:
(x, y, z), (q0, q1, q2, q3), proc = cut_pose
path.poses.append(
PoseStamped(pose=Pose(Point(x / 1000, y / 1000, z /
1000), Quaternion(q0, q1, q2, q3))))
pub.publish(path)
rospy.sleep(2.0)
k = 0
N = len(cut_path)
while not rospy.is_shutdown() and (k < N):
(x, y, z), (q0, q1, q2, q3), proc = cut_path[k]
rospy.loginfo("%s, %s" % (cut_path[k], rospy.get_time()))
pose = PoseStamped(
Header(frame_id=FRAME),
Pose(Point(x / 1000, y / 1000, z / 1000),
Quaternion(q0, q1, q2, q3)))
pub_pose.publish(pose)
k = k + 1
rospy.sleep(1.0)
def talker():
pub = rospy.Publisher('path', Path)
pub_pose = rospy.Publisher('pose', PoseStamped)
pub_traj = rospy.Publisher('joint_path_command', JointTrajectory)
rospy.init_node('path_publisher')
path = Path()
rospy.loginfo(tf.transformations.quaternion_from_euler(0, 0, 0))
#path.header = Header(frame_id='work_object')
path.header = Header(frame_id=FRAME)
#path.poses = [PoseStamped(pose=Pose(Point(0, 0, 0.5), Quaternion(0, 0, 0, 1))),
#PoseStamped(pose=Pose(Point(0, 1, 1.4), Quaternion(0, 0, 0, 1)))]
traj = JointTrajectory()
traj.points = [JointTrajectoryPoint(positions=[1.0, 0.0, 0.0, 0.0, 0.0, 0.0])]
rospy.loginfo(traj)
pub_traj.publish(traj)
layers = dxf.read_layers('/home/jorge/ros_workspace/robot_path/curvas_v16.dxf')
points1, frames1 = dxf2path.get_vectors(layers['Copa1_I'], layers['Copa1_E'])
cut_path = dxf2path.frames2path(points1, frames1)
for cut_pose in cut_path:
(x, y, z), (q0, q1, q2, q3), proc = cut_pose
path.poses.append(PoseStamped(pose=Pose(Point(x/1000, y/1000, z/1000), Quaternion(q0, q1, q2, q3))))
pub.publish(path)
rospy.sleep(2.0)
k = 0
N = len(cut_path)
while not rospy.is_shutdown() and (k < N):
(x, y, z), (q0, q1, q2, q3), proc = cut_path[k]
rospy.loginfo("%s, %s" %(cut_path[k], rospy.get_time()))
pose = PoseStamped(Header(frame_id=FRAME),
Pose(Point(x/1000, y/1000, z/1000),
Quaternion(q0, q1, q2, q3)))
pub_pose.publish(pose)
k = k + 1
rospy.sleep(1.0)
def listener():
global poses
# in ROS, nodes are unique named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'talker' node so that multiple talkers can
# run simultaenously.
rospy.init_node('mapper', anonymous=True)
rospy.Subscriber("odom", PoseStamped, callback)
pub = rospy.Publisher('trajectory', Path, queue_size = 10)
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
path = Path()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = 'base_link'
path.header = h
path.poses = poses
pub.publish(path)
r.sleep()
def path_pub_callback(self, event):
if self.path:
path = Path()
path.header = self.path[-1].header
path.poses = self.path[-30000::1]
self.out_path_pub.publish(path)
#header.seq = int(row[0])
header.seq = index
index = index + 1
stamp = stamp_ref + (float(row[0]) - index_ref) * 1.0 / 48
header.stamp.secs = int(stamp);
header.stamp.nsecs = int((stamp - int(stamp)) * 1000000000)
print header.stamp.secs
header.frame_id = '/world'
ps = PoseStamped()
ps.header = header
p = Pose()
p.position.x = float(row[1])
p.position.y = float(row[2])
p.position.z = float(row[3])
p.orientation.x = float(row[4])
p.orientation.y = float(row[5])
p.orientation.z = float(row[6])
p.orientation.w = float(row[7])
ps.pose = p
path.header = header
path.poses.append(ps)
if header.seq % 100 == 0:
bag.write('sfm_path', path, header.stamp)
bag.write('sfm_pose', ps, header.stamp)
finally:
bag.close()
