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 Lvbo(path,D): mPath=Path() mPath.header.frame_id=path.header.frame_id poses=path.poses P=len(poses) newPoses=[] newPoses.append(poses[0]) print '---------------------------' #如果只有或者小于三个点 if P<4: for i in range(1,P): newPoses.append(poses[i]) mPath.poses=newPoses[:] return mPath #从第三个点开始计算 i=2 FLAG=0 while i<(P-1): d=canculateDistance(poses[i],poses[i-1]) if (d<D) : if d<0.25 and ((abs(poses[i-1].pose.position.x-poses[i].pose.position.x)<0.05) or (abs(poses[i-1].pose.position.y-poses[i].pose.position.y)<0.05)): newPoses.append(poses[i-1]) i+=2 FLAG=3 else: #直线角度若相差过小 可能产生尖端 k1=canculate_G_C_Angle(poses[i-2],poses[i-1]) k2=canculate_G_C_Angle(poses[i],poses[i+1]) #30度以内 if abs(normalize_angle(k1-k2))<0.5: newPoses.append(poses[i-1]) i+=2 FLAG=2 else: result=CrossPoint(poses[i-2],poses[i-1],poses[i],poses[i+1]) poses[i-1].pose.position.x=result[0] poses[i-1].pose.position.y=result[1] newPoses.append(poses[i-1]) i+=2 FLAG=1 else: newPoses.append(poses[i-1]) i+=1 FLAG=2 if i==(P-1): newPoses.append(poses[P-2]) newPoses.append(poses[P-1]) mPath.poses=newPoses[:] return mPath if i==P : newPoses.append(poses[P-1]) mPath.poses=newPoses[:] return mPath
def on_send_char(self):
#<hyin/Apr-12th-2016> add timestamp for each way point. This is desired by the NAOqi SDK
t0 = 2.5
dt = 0.1
#first try to send perturbed data
if self.char_mdl:
msg = MultiPaths()
for curr_idx in range(len(self.char_mdl)):
mdl, t_array, opt_parms, traj_opt, vel_vec_opt, theta_opt = self.get_perturbed_trajectory_and_parms(curr_idx)
tmp_path = Path()
tmp_path.header.frame_id = 'writing_surface'
# tmp_path.header.stamp = rospy.Time.now()
tmp_path.poses = [None] * len(traj_opt)
for idx, pnt in enumerate(traj_opt):
tmp_path.poses[idx] = PoseStamped()
tmp_path.poses[idx].pose.position.x = pnt[0]
tmp_path.poses[idx].pose.position.y = -pnt[1]
tmp_path.poses[idx].header.frame_id = 'writing_surface';
tmp_path.poses[idx].header.stamp = rospy.Time(t0+idx*dt); #assume constant time between points for now
msg.paths.append(tmp_path)
#send this
msg.header.frame_id = 'writing_surface'
msg.header.stamp = rospy.Time.now()
self.ros_publisher.publish(msg)
rospy.loginfo('GAIPS_PYTK_VIEWER: Sent perturbed character data...')
return
#send original data if the perturbed data is not available
curr_data = self.get_current_data()
if curr_data is not None:
msg = MultiPaths()
for strk in curr_data:
tmp_path = Path()
tmp_path.header.frame_id = 'writing_surface'
tmp_path.poses = [None] * len(strk)
for idx, pnt in enumerate(strk):
tmp_path.poses[idx] = PoseStamped()
tmp_path.poses[idx].pose.position.x = pnt[0]
tmp_path.poses[idx].pose.position.y = -pnt[1]
tmp_path.poses[idx].header.frame_id = 'writing_surface';
tmp_path.poses[idx].header.stamp = rospy.Time(t0+idx*dt); #assume constant time between points for now
msg.paths.append(tmp_path)
#send this
msg.header.frame_id = 'writing_surface'
msg.header.stamp = rospy.Time.now()
self.ros_publisher.publish(msg)
rospy.loginfo('GAIPS_PYTK_VIEWER: Sent character data...')
return
def publish_via_points_msg():
pub = rospy.Publisher('/move_base/TebLocalPlannerROS/via_points', Path, queue_size=1)
rospy.init_node("test_via_points_msg")
via_points_msg = Path()
via_points_msg.header.stamp = rospy.Time.now()
via_points_msg.header.frame_id = "map" # CHANGE HERE: odom/map
# Add via-points
point1 = PoseStamped()
point1.pose.position.x = 4.0;
point1.pose.position.y = 1.0;
point2 = PoseStamped()
point2.pose.position.x = 5.0;
point2.pose.position.y = 0.0;
via_points_msg.poses = [point1, point2]
r = rospy.Rate(5) # 10hz
t = 0.0
while not rospy.is_shutdown():
pub.publish(via_points_msg)
r.sleep()
def path_Callback(self):
out = Path()
out.poses = self.path
out.header.frame_id = 'world'
out.header.stamp = rospy.Time.now()
self.pub_path.publish(out)
# def trueMarkerCallback(self):
# markerArray = MarkerArray()
# sphere_rad = .2
# marker1 = Marker()
# marker1.header.stamp = rospy.Time.now()
# marker1.header.frame_id = "/world"
# marker1.id = detect.object_id
# marker1.type = marker1.SQUARE
# marker1.action = marker1.ADD
# marker1.pose.position.x = self.pos[0]
# marker1.pose.position.y = self.pos[1]
# marker1.pose.position.z = self.pos[2]
# marker1.pose.orientation.w = 1
# marker1.scale.x = sphere_rad*2
# marker1.scale.y = sphere_rad*2
# marker1.scale.z = sphere_rad*2
# marker1.color.r = 1.0
# marker1.color.g = 0
# marker1.color.b = 1.0
# marker1.color.a = 0.7
# marker1.lifetime = rospy.Duration.from_sec(0)
# marker1.frame_locked = 0
# markerArray.markers.append(marker1)
self.pub.publish(markerArray)
def talker():
path_pub = rospy.Publisher('/ardrone/path_pub', Path, queue_size=10)
pos_pub = rospy.Publisher('/ardrone/pos_pub', PoseStamped, queue_size=10)
rospy.init_node('path_generator', anonymous=True)
freq = 100;
round_min = 0.1;
path_msg = Path();
path_msg.header.stamp = rospy.get_rostime();
path_msg.poses = [];
path_msg.header.frame_id = "nav";
radius = 2.0;
rate = rospy.Rate(freq) # 100hz
count = 0;
while not rospy.is_shutdown():
count += 1;
pos_msg = PoseStamped();
pos_msg.header.stamp = rospy.get_rostime();
pos_msg.header.frame_id = "nav";
pos_msg.pose.position.x = radius * np.cos((np.pi * 2) / freq * round_min * count);
pos_msg.pose.position.y = radius * np.sin((np.pi * 2) / freq * round_min * count);
pos_msg.pose.position.z = 1.5;
if count > (freq / round_min):
path_msg.poses.pop(0)
path_msg.poses.append(pos_msg);
path_pub.publish(path_msg)
pos_pub.publish(pos_msg)
rate.sleep()
def get_path_message(self):
path_msg = Path()
try:
path_msg.header.stamp = self.node.get_clock().now().to_msg()
set_timestamps = True
except:
set_timestamps = False
self._logger.warning('ROS node was not initialized, no timestamp '
'can be assigned to path message')
path_msg.header.frame_id = 'world'
path_msg.poses = list()
for point in self._local_planner.points:
pose = PoseStamped()
if set_timestamps:
(secs, nsecs) = float_sec_to_int_sec_nano(point.t)
pose.header.stamp = rclpy.time.Time(seconds=secs,
nanoseconds=nsecs)
pose.pose.position = Vector3(x=point.p[0],
y=point.p[1],
z=point.p[2])
pose.pose.orientation = Quaternion(x=point.q[0],
y=point.q[1],
z=point.q[2],
w=point.q[3])
path_msg.poses.append(pose)
def talk(body_id):
loop_rate=rospy.Rate(30)
position=rospy.Publisher('rviz/quad_position',Marker,queue_size=10)
trajectory=rospy.Publisher('rviz/quad_trajectory',Path,queue_size=10)
#wait for the mocap services to be up and running
try:
rospy.loginfo('Connecting to the mocap system...')
rospy.wait_for_service('mocap_get_data',10)
except:
rospy.logerr('[RVIZ] No connection to the mocap system')
sys.exit()
rospy.loginfo('[RVIZ] Connection established')
path=Path()
path.header.stamp=rospy.get_rostime()
path.header.frame_id='map'
posestamped=PoseStamped()
posestamped.header.stamp=rospy.get_rostime()
posestamped.header.frame_id='map'
posestamped.pose.position.x=0.0
posestamped.pose.position.y=0.0
posestamped.pose.position.z=0.0
list_posestamped=[posestamped]
del list_posestamped[0]
while not rospy.is_shutdown():
#try to contact the mocap service, exit the program if there is no connection
try:
body_info=rospy.ServiceProxy('mocap_get_data',BodyData)
body=body_info(body_id)
except:
rospy.logerr('[RVIZ] No connection to the mocap system')
sys.exit()
#distinguish between the case where a body has been found, and when it hasn't
if body.pos.found_body:
data_to_rviz=draw_body_found(body)
#add the current point to the trajectory line
current_point=PoseStamped()
current_point.pose.position.x=data_to_rviz.pose.position.x
current_point.pose.position.y=data_to_rviz.pose.position.y
current_point.pose.position.z=data_to_rviz.pose.position.z
list_posestamped.append(current_point)
else:
rospy.logerr('[RVIZ] The requested body is not available')
data_to_rviz=draw_body_error(body)
#publish the data to rviz
path.poses=list_posestamped
#position and orientation of the quad
position.publish(data_to_rviz)
#trajectory of the quad
trajectory.publish(path)
loop_rate.sleep()
def publish_path(self, plan, stamp, pub):
"""
Broadcast our waypoints
"""
if not rospy.is_shutdown() and pub.get_num_connections() > 0:
path_msg = PathRosMsg()
path_msg.header.stamp = stamp
path_msg.header.frame_id = self.own_map_frame_id
if plan:
path_msg.poses = [
PoseStamped() for i in range(len(plan.nodes))
]
for i in range(len(plan.nodes)):
pose = PoseStamped()
node = plan.nodes[i]
pose.header.frame_id = self.own_map_frame_id
pose.header.stamp = node.stamp
pose.pose.position.x = node.x
pose.pose.position.y = node.y
pose.pose.position.z = 0.0 # TODO(marcus): extend to 3D
pose.pose.orientation.w = 1.0 # TODO(marcus): include orientation info
path_msg.poses[i] = pose
try:
pub.publish(path_msg)
except rospy.ROSException:
pass
def ros_path_from_map_path(map_path, start_pose, end_pose, resolution,
frame_id):
ros_path = RosPath()
ros_path.header.seq = 1
ros_path.header.stamp = rospy.Time.now()
ros_path.header.frame_id = frame_id
poses = []
# If path is not empty, add poses, otherwise ros path stays empty
if map_path:
start_pose.header.seq = 1
poses = [start_pose]
id_counter = 2
previous_pose = start_pose
for point in map_path[1:len(map_path) - 1]:
new_pose = Utils.ros_pose_from_map_coord_yaw(
point[0], point[1], 0.0, resolution, id_counter, frame_id)
direction_vector = (new_pose.pose.position.x -
previous_pose.pose.position.x,
new_pose.pose.position.y -
previous_pose.pose.position.y)
new_pose.pose.orientation = Utils.geom_quat_from_yaw(
Utils.yaw_from_direction(direction_vector))
id_counter = id_counter + 1
previous_pose = new_pose
poses.append(new_pose)
end_pose.header.seq = id_counter
poses.append(end_pose)
ros_path.poses = poses
return ros_path
def ros_path_from_poses(start_pose, end_pose, frame_id,
push_pose_unit_direction_vector,
push_unit_distance):
start_pose.header.seq = 1
ros_path = RosPath()
ros_path.header.seq = 1
ros_path.header.stamp = rospy.Time.now()
ros_path.header.frame_id = frame_id
ros_path.poses = [start_pose]
distance_between_poses = Utils.euclidean_distance_ros_poses(
start_pose, end_pose)
current_distance = push_unit_distance
new_pose = copy.deepcopy(start_pose)
while current_distance <= distance_between_poses:
new_pose = copy.deepcopy(new_pose)
new_pose.header.seq = new_pose.header.seq + 1
new_pose.pose.position.x = new_pose.pose.position.x + push_pose_unit_direction_vector[
0] * push_unit_distance
new_pose.pose.position.y = new_pose.pose.position.y + push_pose_unit_direction_vector[
1] * push_unit_distance
ros_path.poses.append(new_pose)
current_distance = current_distance + push_unit_distance
end_pose.header.seq = new_pose.header.seq + 1
ros_path.poses.append(end_pose)
return ros_path
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 publishCollFreeTraj(self, time_stamp_current):
if (self.obstacle_avoidance.flag_set_robot_traj_coll_free_ref == True):
coll_free_robot_traj_msg = Path()
coll_free_robot_traj_msg.header.stamp = time_stamp_current
coll_free_robot_traj_msg.header.frame_id = self.world_frame
coll_free_robot_traj_msg.poses = []
for pose_i in self.obstacle_avoidance.robot_traj_coll_free_ref:
pose_i_msg = PoseStamped()
pose_i_msg.header.stamp = rospy.Time()
pose_i_msg.header.frame_id = self.world_frame
pose_i_msg.pose.position.x = pose_i.position[0]
pose_i_msg.pose.position.y = pose_i.position[1]
pose_i_msg.pose.position.z = pose_i.position[2]
pose_i_msg.pose.orientation.w = pose_i.attitude_quat_simp[0]
pose_i_msg.pose.orientation.x = 0.0
pose_i_msg.pose.orientation.y = 0.0
pose_i_msg.pose.orientation.z = pose_i.attitude_quat_simp[1]
coll_free_robot_traj_msg.poses.append(pose_i_msg)
#
self.coll_free_robot_traj_pub.publish(coll_free_robot_traj_msg)
#
return
def plan_callback(msg):
global first_planner_flag
if first_planner_flag:
global nav_path
nav_path = Path()
nav_path.poses = msg.poses
first_planner_flag = False
def target_path(self, fname):
poses = []
points = []
with open(fname) as wfile:
reader = csv.DictReader(wfile,
delimiter=' ',
fieldnames=CSV_HEADER)
for index, wp in enumerate(reader):
pose_stamped = PoseStamped()
pose = Pose()
pose.position.x = float(wp['x'])
pose.position.y = float(wp['y'])
pose.position.z = 0.0
pose_stamped.header.frame_id = self.frame_id
pose_stamped.header.seq = index
pose_stamped.pose = pose
poses.append(pose_stamped)
path = Path()
path.header.frame_id = self.frame_id
path.header.stamp = rospy.Time(0)
path.poses = poses
return path
def random_path_publisher():
pub = rospy.Publisher('path_to_follow', Path, queue_size=10)
rospy.init_node('random_planner', anonymous=True)
rate = rospy.Rate(1) # 10hz
rate.sleep()
#hello_str = "hello world %s" % rospy.get_time()
myPath = Path()
myPath.header.frame_id="odom"
myPath.header.stamp = rospy.Time.now()
myPoses = []
for i in range(POINTS):
newPose = PoseStamped()
newPose.header.frame_id="odom"
newPose.header.stamp = rospy.Time.now()
newPose.pose.position = Point(random.uniform(-SPREAD,SPREAD), random.uniform(-SPREAD,SPREAD), 0)
myPoses.append(newPose)
myPath.poses = myPoses
#pub2.publish(myPoses[0])
rospy.loginfo(myPath)
pub.publish(myPath)
rate.sleep()
rospy.spin()
def as_path(self, dt, start_time, frame='odom'):
if self.trajectory is None:
return None
ts = np.arange(0, self.trajectory.end_time(), dt)
poses = []
for t in ts:
pos = self.val(t)
vel = self.val(t, order=1)
pose = PoseStamped()
pose.header.frame_id = frame
pose.header.stamp = start_time + rospy.Duration(t)
pose.pose.position.x = pos[0]
pose.pose.position.y = pos[1]
pose.pose.position.z = pos[2]
vel = np.array(vel) / np.linalg.norm(np.array(vel))
psi = atan2(vel[1], vel[0])
theta = asin(-vel[2])
q = Rotation.from_euler('ZYX', [psi, theta, 0]).as_quat()
pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
poses.append(pose)
path = Path()
path.header.frame_id = frame
path.header.stamp = start_time
path.poses = poses
return path
def direction_callback(self, msg):
direction = msg.data
possibities = {"L":-1, "S":0, "R":+1}
if not direction in possibities.keys():
rospy.logfatal("Invalid intersection type !")
return False
# Determine path points.
self.path_points = path_generate(possibities[direction],
self.n_path_points)
# Set direction dependent parameters.
if(direction=='S'):
la_dis=self.la_dis_straight
target_vel = self.target_vel_straight
if(direction=='L'):
la_dis=self.la_dis_left
target_vel = self.target_vel_left
if(direction=='R'):
la_dis=self.la_dis_right
target_vel = self.target_vel_right
self.controller = Controller(direction,self.path_points,self.wheel_distance,
self.adm_error, la_dis, self.min_radius, target_vel, self.n_hist)
# Publish path.
path = Path()
path.header.frame_id = self.world_frame
path.poses = []
for point in self.path_points:
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = self.world_frame
pose_stamped.pose.position.x = point[0]
pose_stamped.pose.position.y = point[1]
path.poses.append(pose_stamped)
self.path_pub.publish(path)
self.direction_sub.unregister()
return True
def waypoints():
pub = rospy.Publisher('waypoints_out', Path, queue_size=10)
rospy.init_node('waypoints', anonymous=True)
rate = rospy.Rate(1) # 10hz
pose = PoseStamped()
pose1 = PoseStamped()
path = Path()
path.poses = []
h = std_msgs.msg.Header()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 0
pose.pose.orientation.x = 0
h.stamp = rospy.Time.now()
pose.header = h
path.poses.append(pose)
pose1.pose.position.x = 0
pose1.pose.position.y = 1
pose1.pose.position.z = 0
pose1.pose.orientation.x = 0
h.stamp = rospy.Time.now()
pose1.header = h
path.poses.append(pose1)
while not rospy.is_shutdown():
pub.publish(path)
rate.sleep()
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 publish_path(self, msg):
""" Record the path taken, but only at a certain rate.
Parameters:
msg -- The Odometry message data.
"""
publishRate = 0.2
currentTime = rospy.get_rostime()
if self.lastPathPublishTime.to_sec(
) + publishRate <= currentTime.to_sec():
# Add to raw path with a timestamped pose from odometers.
poseStamped = PoseStamped()
poseStamped.header.frame_id = rospy.get_param(
rospy.search_param('sub_kobuki_odom'))
poseStamped.header.stamp = currentTime
poseStamped.pose = msg.pose.pose
self.rawPath += [poseStamped]
# Create and publish the path.
path = Path()
path.header.frame_id = rospy.get_param(
rospy.search_param('sub_kobuki_odom'))
path.header.stamp = currentTime
path.poses = self.rawPath
self.pubPath.publish(path)
self.lastPathPublishTime = currentTime
def construct_path_object(frame_id, waypoints):
""" Path object contains the list of final paths ahead with velocity"""
path = Path()
path.header.frame_id = frame_id
path.poses = waypoints
path.header.stamp = rospy.Time.now()
return path
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 to_path(self, x, start_time, frame='odom'):
assert x.shape[0] == self.xdim
assert x.shape[0] == 8
num_poses = x.shape[1]
poses = []
for i in range(num_poses):
t = start_time + rospy.Duration(self.dt * i)
p = PoseStamped()
p.pose.position = Point(x=x[0, i], y=x[1, i], z=x[2, i])
vel = np.array([x[4, i], x[5, i], x[6, i]])
if abs(linalg.norm(vel)) > 1e-8:
vel = vel / linalg.norm(vel)
psi = atan2(vel[1], vel[0])
theta = asin(-vel[2])
q = Rotation.from_euler('ZYX', [psi, theta, 0]).as_quat()
p.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
p.header.frame_id = frame
p.header.stamp = t
poses.append(p)
path = Path()
path.header.frame_id = frame
path.header.stamp = start_time
path.poses = poses
return path
def getWaypoints(self, list_of_gridpos, goal_pose, publisher=None):
resolution = self.og.info.resolution
width = self.og.info.width
height = self.og.info.height
offsetX = self.og.info.origin.position.x
offsetY = self.og.info.origin.position.y
wp = Path()
wp.header.frame_id = self.og.header.frame_id
poses = []
lastdirection = -999
for i in range(0, len(list_of_gridpos) - 1):
direction = getDirection(list_of_gridpos[i], list_of_gridpos[i + 1])
if direction != lastdirection:
lastdirection = copy.deepcopy(direction)
newPose = PoseStamped()
newPose.header.frame_id = self.og.header.frame_id
newPose.pose = Pose()
newPose.pose.position = getPoint(list_of_gridpos[i], resolution, offsetX, offsetY)
quaternion = tf.transformations.quaternion_from_euler(0, 0, direction)
newPose.pose.orientation.x = quaternion[0]
newPose.pose.orientation.y = quaternion[1]
newPose.pose.orientation.z = quaternion[2]
newPose.pose.orientation.w = quaternion[3]
poses.append(newPose)
newPose = self.tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(goal_pose))
poses.append(newPose)
wp.poses = poses
if publisher is not None:
publisher.publish(wp)
return wp.poses
def publishWaypoints(list_of_gridpos):
global goal_pose
global pubrealpath
wp = Path()
wp.header.frame_id = 'map'
poses = []
lastdirection = -999
for i in range(0, len(list_of_gridpos) - 1):
direction = getDirection(list_of_gridpos[i], list_of_gridpos[i + 1])
if direction != lastdirection:
lastdirection = copy.deepcopy(direction)
newPose = PoseStamped()
newPose.header.frame_id = 'map'
newPose.pose = Pose()
newPose.pose.position = getPoint(list_of_gridpos[i])
quaternion = tf.transformations.quaternion_from_euler(
0, 0, direction)
newPose.pose.orientation.x = quaternion[0]
newPose.pose.orientation.y = quaternion[1]
newPose.pose.orientation.z = quaternion[2]
newPose.pose.orientation.w = quaternion[3]
poses.append(newPose)
newPose = PoseStamped()
newPose.header.frame_id = 'map'
newPose.pose = goal_pose
poses.append(newPose)
wp.poses = poses
pubrealpath.publish(wp)
return wp
def publish_via_points_msg():
pub = rospy.Publisher('/test_optim_node/via_points', Path, queue_size=1)
rospy.init_node("test_via_points_msg")
via_points_msg = Path()
via_points_msg.header.stamp = rospy.Time.now()
via_points_msg.header.frame_id = "map" # CHANGE HERE: odom/map
# Add via-points
point1 = PoseStamped()
point1.pose.position.x = 0.0
point1.pose.position.y = 1.5
point2 = PoseStamped()
point2.pose.position.x = 2.0
point2.pose.position.y = -0.5
via_points_msg.poses = [point1, point2]
r = rospy.Rate(5) # 10hz
t = 0.0
while not rospy.is_shutdown():
pub.publish(via_points_msg)
r.sleep()
def path_callback(self, map):
self.map = map
if self.firstCall == False:
rospy.loginfo("+++++++++++++++++initX: " + str(self.initx) +
" initY: " + str(self.inity) + " goalX: " +
str(self.goalx) + " goalY: " + str(self.goaly))
self.firstCall = True
if self.world != 0:
dijkstra = Dijkstra(self.map)
self.path = dijkstra.planning(self.initx, self.inity,
self.goalx, self.goaly)
self.save_as_yaml(self.path)
x, y = self.path
x = x[::-1]
y = y[::-1]
my_path = Path()
my_path.header.frame_id = "map"
my_path.header.stamp = rospy.get_rostime()
size = range(len(self.path[0]))
poses = []
for i in size:
p_stamp = PoseStamped()
p_stamp.pose.position.x = x[i]
p_stamp.pose.position.y = y[i]
p_stamp.pose.position.z = 0
p_stamp.header.frame_id = "map"
p_stamp.header.stamp = rospy.get_rostime()
poses.append(p_stamp)
my_path.poses = poses
self.pathPublisher.publish(my_path)
else:
poses = []
self.path = [(self.goalx, self.goaly)]
my_path = Path()
my_path.header.frame_id = "map"
my_path.header.stamp = rospy.get_rostime()
for i in range(2):
p_stamp = PoseStamped()
p_stamp.pose.position.x = self.goalx
p_stamp.pose.position.y = self.goaly
p_stamp.pose.position.z = 0
p_stamp.header.frame_id = "map"
p_stamp.header.stamp = rospy.get_rostime()
poses.append(p_stamp)
my_path.poses = poses
self.pathPublisher.publish(my_path)
def talk(body_id):
loop_rate = rospy.Rate(30)
position = rospy.Publisher('rviz/quad_position', Marker, queue_size=10)
trajectory = rospy.Publisher('rviz/quad_trajectory', Path, queue_size=10)
#wait for the mocap services to be up and running
try:
rospy.loginfo('Connecting to the mocap system...')
rospy.wait_for_service('mocap_get_data', 10)
except:
rospy.logerr('[RVIZ] No connection to the mocap system')
sys.exit()
rospy.loginfo('[RVIZ] Connection established')
path = Path()
path.header.stamp = rospy.get_rostime()
path.header.frame_id = 'map'
posestamped = PoseStamped()
posestamped.header.stamp = rospy.get_rostime()
posestamped.header.frame_id = 'map'
posestamped.pose.position.x = 0.0
posestamped.pose.position.y = 0.0
posestamped.pose.position.z = 0.0
list_posestamped = [posestamped]
del list_posestamped[0]
while not rospy.is_shutdown():
#try to contact the mocap service, exit the program if there is no connection
try:
body_info = rospy.ServiceProxy('mocap_get_data', BodyData)
body = body_info(body_id)
except:
rospy.logerr('[RVIZ] No connection to the mocap system')
sys.exit()
#distinguish between the case where a body has been found, and when it hasn't
if body.pos.found_body:
data_to_rviz = draw_body_found(body)
#add the current point to the trajectory line
current_point = PoseStamped()
current_point.pose.position.x = data_to_rviz.pose.position.x
current_point.pose.position.y = data_to_rviz.pose.position.y
current_point.pose.position.z = data_to_rviz.pose.position.z
list_posestamped.append(current_point)
else:
rospy.logerr('[RVIZ] The requested body is not available')
data_to_rviz = draw_body_error(body)
#publish the data to rviz
path.poses = list_posestamped
#position and orientation of the quad
position.publish(data_to_rviz)
#trajectory of the quad
trajectory.publish(path)
loop_rate.sleep()
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 find_new_path(self, start, goal):
path_plan = Path(Header(0, rospy.Time(0), "/map"), [])
while (len(path_plan.poses) is 0):
if '/amcl' in self.nodes:
self.reset_amcl(start.pose)
self.srv_clear_costmaps()
path_plan.poses = self.srv_make_plan(start, goal, 0.1).plan.poses
return path_plan
def publish_path(self, ps):
path = Path()
path.header.frame_id = self.camera_frame_id
path.header.stamp = ps.header.stamp
self.path_list.append(ps)
path.poses = self.path_list
self.path_pub.publish(path)
return
def __find_new_path__(self,start,goal):
path_plan = Path(Header(0,rospy.Time.now(),"map"),[])
while(len(path_plan.poses) is 0):
self.scl_clear_costmaps()
ps_start = PoseStamped(Header(0,rospy.Time.now(),"map"), start)
ps_goal = PoseStamped(Header(0,rospy.Time.now(),"map"), goal)
path_plan.poses = self.scl_make_plan(ps_start, ps_goal, 0.1).plan.poses
return path_plan
def publish_path(self, ps):
path = Path()
path.header.frame_id = self.camera_frame_id
path.header.stamp = ps.header.stamp
self.path_list.append(ps)
path.poses = self.path_list
self.path_pub.publish(path)
return
def path_timercb(self, time_dat):
if len(self.mass_ref_vec) > 0:
# send reference path
ref_path = Path()
ref_path.header.stamp = time_dat.current_real
ref_path.header.frame_id = self.mass_ref_vec[-1].header.frame_id
ref_path.poses = list(self.mass_ref_vec)
self.ref_path_pub.publish(ref_path)
if len(self.mass_filt_vec) > 0:
# send filtered path
filt_path = Path()
filt_path.header.stamp = time_dat.current_real
filt_path.header.frame_id = self.mass_filt_vec[-1].header.frame_id
filt_path.poses = list(self.mass_filt_vec)
self.filt_path_pub.publish(filt_path)
return
def broadcast(self, msg):
if len(self.currentPath):
# Publish the path
cmd = Path()
cmd.poses = self.currentPath
cmd.header.frame_id = 'map'
self.pub_path.publish(cmd)
# Prep the next path portion
self.currentPath = self.nextPathSegment
self.nextPathSegment = []
def get_path_message(self):
path_msg = Path()
path_msg.header.stamp = rospy.Time.now()
path_msg.header.frame_id = 'world'
path_msg.poses = list()
for point in self._local_planner.points:
pose = PoseStamped()
pose.header.stamp = rospy.Time(point.t)
pose.pose.position = Vector3(*point.p)
pose.pose.orientation = Quaternion(*point.q)
path_msg.poses.append(pose)
def callback(odometry):
stampedPose = PoseStamped()
stampedPose.pose = odometry.pose.pose
__poses__.append(stampedPose)
msg = Path()
msg.header.frame_id = 'odom'
msg.header.stamp = rospy.Time.now()
msg.poses = __poses__
__path_publisher__.publish(msg)
if __enable_logs__:
rospy.loginfo('Sent PATH: \n{}'.format(odometry.pose.pose))
def publishPath(self):
msg = Path()
msg.header.frame_id = self.frame_id
poses = []
for node in self.path:
x,y = self.convert_node_to_point(node)
pose_msg = PoseStamped()
pose_msg.header.frame_id = self.frame_id
pose_msg.pose.position.x = x
pose_msg.pose.position.y = y
poses += [pose_msg]
msg.poses = poses
self.pub_path.publish(msg)
def publish_path(self, edges):
# Send the path to the path_follower node
cmd = Path()
poses = np.concatenate([
edge.interpolate(step=0.2)[:-1]
for edge in edges
] + [[edges[-1].dest]])
cmd.poses = [
rrtutils.pose_for_node(pose) for pose in poses
]
cmd.header.frame_id = self.map.frame
#rospy.loginfo("New Poses {}.".format(cmd.poses))
self.pub_path.publish(cmd)
def timer_callback(self, event):
if self.path is None:
rospy.logwarn("rrt planner: No goal")
return
if not self.goal_changed:
return
self.goal_changed = False
path = Path()
path.header.frame_id = 'map'
path.poses = self.path
self.pub_path.publish(path)
rospy.loginfo('Planned!')
def GetPath (gridMap, start, goal): parents, costs = SearchForGoal(gridMap, start, goal) path = Path() path.poses = [PoseStamped()] currentIndex = 0 currentNode = goal while not IsSame(currentNode, start): path.poses[currentIndex].pose.position = currentNode currentNode = parents[currentNode] currentIndex += 1 path.poses[currentIndex].pose.position = start return path
def mapCallback(msg): global frontier_pub global viz_pub print "Got Map" time = rospy.get_time() res = msg.info.resolution width = msg.info.width height = msg.info.height data = msg.data frontiers = [] seen = [] for cell in range(width*height): p = denormalize(cell, width) if data[cell] > -1 and p not in seen: seen.append(p) val = isEdge(p, msg) if (val > 0): stack = [] front = [Point(p.x,p.y,val)] for n in getNeighbors(p, msg): if n not in seen: val = isEdge(n, msg) if (val > 0): stack.append(n) front.append(Point(n.x, n.y, val)) seen.append(n) while (len(stack) > 0): np = stack.pop() for n in getNeighbors(np, msg): if n not in seen: val = isEdge(n, msg) if (val > 0): stack.append(n) front.append(Point(n.x, n.y, val)) seen.append(n) frontiers.append(front) centroids = map(lambda x: centroid(msg, x), frontiers) resp = GridCells() resp.cell_width = res resp.cell_height = res resp.header.frame_id = 'map' resp.cells = map(lambda x: x.pose.position, centroids) viz_pub.publish(resp) path = Path() path.poses = centroids frontier_pub.publish(path) print "Time:", (rospy.get_time()-time)
def run(self):
point1 = PoseStamped()
point1.pose.position.x = POINT_1_X
point1.pose.position.y = POINT_1_Y
point2 = PoseStamped()
point2.pose.position.x = POINT_2_X
point2.pose.position.y = POINT_2_Y
path = Path()
path.poses = []
path.poses.append(point1)
path.poses.append(point2)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
self.path_pub.publish(path)
rate.sleep()
rospy.spin()
def emitPath(ident, pointList):
pub = rospy.Publisher("path/"+str(ident), Path, queue_size=1)
path = Path()
path.header.frame_id = "map"
def point2ps(point):
ps = PoseStamped()
ps.header.frame_id = path.header.frame_id
ps.pose.position = point
return ps
path.poses = [point2ps(p) for p in pointList]
def path_publisher():
while not rospy.is_shutdown():
pub.publish(path)
rospy.sleep(1)
t = Thread(target=path_publisher)
t.start()
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 ChooseMainPath(path): mPath=Path() mPath.header.frame_id=path.header.frame_id poses=path.poses P=len(poses) if P<3: return path newPoses=[] newPoses.append(poses[0]) lastAngle=canculate_G_C_Angle(poses[1],poses[0]) for i in range(1,P-1): angle=canculate_G_C_Angle(poses[i+1],poses[i]) #print 'agnle-' errA=abs(angle-lastAngle) if errA>6: errA=6.283-errA #print errA if errA>0.1: newPoses.append(poses[i]) lastAngle=angle newPoses.append(poses[P-1]) mPath.poses=newPoses[:] return mPath
def getWaypoints(initPos, goalPos, grid):
startCell = convertPointToCell(initPos, grid.origin, grid.resolution)
goalCell = convertPointToCell(goalPos, grid.origin, grid.resolution)
#Logic that gets the robot out of an obstacle (failure recovery)
if grid.getCellValue(startCell) == CellType.Obstacle:
print "The robot is inside the obstacle! Trying to find the shortest way out..."
cellTypes = set()
cellTypes.add(CellType.Empty)
cellTypes.add(CellType.Unexplored)
pathOutOfObstacle = PathFinder.findPathToCellWithValueClosestTo(grid, startCell, cellTypes, goalCell)
startCell = pathOutOfObstacle.pop(len(pathOutOfObstacle) - 1)
pathFinder = PathFinder(startCell, goalCell)
if (startCell == goalCell):
pathFinder.waypoints.append(goalCell)
else:
aStarDone = False
while not aStarDone:
aStarDone = pathFinder.runAStarIteration(grid)
publishGridCells(expanded_cell_pub, pathFinder.expanded, grid.resolution, grid.cellOrigin)
#Convert frontier queue to the frontier set
frontierCells = set()
for tuple in pathFinder.frontier.elements:
cell = tuple[1]
if cell not in pathFinder.expanded and cell not in frontierCells:
frontierCells.add(cell)
publishGridCells(frontier_cell_pub, frontierCells, grid.resolution, grid.cellOrigin)
pathFinder.findPath()
#if robot was at an obstacle cell before, then prepend the path out of the obstacle
try:
pathOutOfObstacle
except NameError:
pass
else:
pathOutOfObstacle.extend(pathFinder.path)
pathFinder.path = pathOutOfObstacle
publishGridCells(path_cell_pub, pathFinder.path, grid.resolution, grid.cellOrigin)
pathFinder.calculateWaypoints()
#convert the waypoints to the trajectory offsets:
waypoints = Path()
waypoints.poses = []
for cell in pathFinder.waypoints:
poseObj = PoseStamped()
poseObj.pose.position = convertCellToPoint(cell, grid.cellOrigin, grid.resolution)
waypoints.poses.append(poseObj)
return waypoints
# send it
pub_steer.publish(v)
# tell the world
#rospy.loginfo('Got e = %f ctrl=%f', e, u)
# start the feedback
pose_sub = rospy.Subscriber('base_pose_ground_truth', Odometry, ctrl_callback)
# and external speed control
speed_sub = rospy.Subscriber('cmd_speed', Float64, speed_callback)
# and laser stopping
speed_sub = rospy.Subscriber('base_scan', LaserScan, laser_callback)
# and the DMS planner
dms_sub = rospy.Subscriber('dms_plan', DMSPlan, dms_callback)
# local copy of current path for publishing
my_current_path = Path()
my_current_path.header.frame_id='world'
while not rospy.is_shutdown():
if dms_plan.path.poses:
current_window = range(len(my_path.poses))
else:
current_window = [kk % num_points for kk in range(last_index, last_index+window_len)]
my_current_path.poses = [my_path.poses[kk] for kk in current_window]
pub_path.publish(my_current_path)
my_rate.sleep()
def move_robot(self):
inp=[self.x0,self.y0,self.xf,self.yf,self.vx0,self.ax0,self.vxf,self.k0,self.kdot0,self.kddot0,self.kf,self.kdotf,self.t0,self.tf,self.xc,self.yc,self.tc];
[xo,x1,x2,x3,x4,xf,ko,k1,k2,k3,k4,kf ] = get_way_points(inp);
print [xo,x1,x2,x3,x4,xf,ko,k1,k2,k3,k4,kf ]
t=self.t0;
i=0;
i_pert=0;
npath=21;
perturbation_at_time=4.1;
delt = self.planner_rate
robot_end = 0;
t_start_pert = 0;
time = zeros((self.tf-self.t0)/delt + 1);
xrobo = zeros((self.tf-self.t0)/delt + 1);
yrobo = zeros((self.tf-self.t0)/delt + 1);
wrobo = zeros((self.tf-self.t0)/delt + 1);
krobo = zeros((self.tf-self.t0)/delt + 1);
xrobodot =zeros((self.tf-self.t0)/delt + 1);
yrobodot = zeros((self.tf-self.t0)/delt + 1);
krobodot = zeros((self.tf-self.t0)/delt + 1);
xroboddot = zeros((self.tf-self.t0)/delt + 1);
kroboddot = zeros((self.tf-self.t0)/delt + 1);
wrobo1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
vrobo1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
xrobo1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
yrobo1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
xrobodot1 = zeros(((self.tf- perturbation_at_time)/delt + 1,npath));
xroboddot1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
yrobodot1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
krobo1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
krobodot1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
kroboddot1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
time_perturb_duration = zeros((self.tf-perturbation_at_time)/delt + 1);
l = []
print "NOW STARTED"
while(t <= self.tf):
time[i] = t
output_robo = get_robot_pose( self.t0,t,self.tf,xo,x1,x2,x3,x4,xf,ko,k1,k2,k3,k4,kf,self.y0);
xrobo[i]=output_robo[0];yrobo[i]=output_robo[1];krobo[i]=output_robo[2];xrobodot[i]=output_robo[3];yrobodot[i]=output_robo[4];krobodot[i]=output_robo[5];xroboddot[i]=output_robo[6];kroboddot[i]=output_robo[7];
wrobo[i] = krobodot[i]/(1.0+krobo[i]**2)
if(abs(t-perturbation_at_time)<0.00000111):
t_start_pert=t;
robo_end=i;
bernp=pert_traj(t_start_pert,self.tf,xrobo[robo_end],yrobo[robo_end],self.xf,self.yf,xrobodot[robo_end],xroboddot[robo_end],yrobodot[robo_end],krobo[robo_end],krobodot[robo_end],kroboddot[robo_end],npath);
if(t > perturbation_at_time ):
time_perturb_duration[i_pert] = t
output_robo_pert = numpy.zeros((npath,8))
for j in range(0,npath):
# print "here"
output_robo_pert[j] = get_robot_pose(t_start_pert,t,self.tf,bernp[0,j],bernp[1,j],bernp[2,j],bernp[3,j],bernp[4,j],bernp[5,j],bernp[6,j],bernp[7,j],bernp[8,j],bernp[9,j],bernp[10,j],bernp[11,j],yrobo[robo_end]);
# print "done"
xrobo1[i_pert]=output_robo_pert[:,0];yrobo1[i_pert]=output_robo_pert[:,1];krobo1[i_pert]=output_robo_pert[:,2];xrobodot1[i_pert]=output_robo_pert[:,3];yrobodot1[i_pert]=output_robo_pert[:,4];krobodot1[i_pert]=output_robo_pert[:,5];xroboddot1[i_pert]=output_robo_pert[:,6];kroboddot1[i_pert]=output_robo_pert[:,7];
# xrobo1[i_pert] = l[1]
# yrobo1[i_pert] = l[2]
# xrobodot1[i_pert] = l[3]
# xroboddot1[i_pert] = l[4]
# yrobodot1[i_pert] = l[5]
# krobo1[i_pert] = l[6]
# krobodot1[i_pert] = l[7]
wrobo1[i_pert]= [krobodot1[i_pert,j]/(1.0+krobo1[i_pert,j]**2) for j in range(0,npath)]
vrobo1[i_pert]=[math.sqrt(xrobodot1[i_pert,j]**2+yrobodot1[i_pert,j]**2) for j in range(0,npath)]
i_pert=i_pert+1;
i = i+1
t = t+delt
r =rospy.Rate(self.rate)
# print self.yc
temp_c = 0;
path_toBe_executed = 10;
while not rospy.is_shutdown():
for j in range(0,21):
if j != 10:
continue;
# if j != path_toBe_executed:
# continue;
# l = calc_scale_single(xrobo1[perturbation_at_time,j],yrobo1[perturbation_at_time,j],xrobodot1[perturbation_at_time,j],yrobodot1[perturbation_at_time,j],3,3,-1,0,2.0);
if temp_c == 0:
l = calc_scale_single(xrobo1[2,j],yrobo1[2,j],xrobodot1[2,j],yrobodot1[2,j],1,8,0,1,2.0);
print l
path = Path()
path.header.frame_id ='map'
path.poses = [PoseStamped]*int((self.tf-self.t0)/delt+1)
s=0
for k in range(0,int((perturbation_at_time-self.t0)/delt)+1):
temp = PoseStamped()
# temp.position.x = xrobo1[k,j];
# temp.position.y = yrobo1[k,j];
temp.header.frame_id ='map'
temp.pose.position.x = xrobo[k];
temp.pose.position.y = yrobo[k];
temp.pose.position.z = 0.1
path.poses[k] = temp
s=s+1
for k in range(0,i_pert):
# print 'hi'
temp = PoseStamped()
# temp.position.x = xrobo1[k,j];
# temp.position.y = yrobo1[k,j];
temp.header.frame_id ='map'
temp.pose.position.x = xrobo1[k,j];
temp.pose.position.y = yrobo1[k,j];
temp.pose.position.z = 0.1
path.poses[k+s] = temp
self.paths_publisher[j].publish(path)
temp_c = temp_c +1
break;
r.sleep()
r2 = rospy.Rate(1.0/delt)
print rospy.Time.now().secs
for i in range (0,int((perturbation_at_time-self.t0)/delt)+1):
vel_command = Twist()
vel_command.linear.x = math.sqrt(xrobodot[i]**2+yrobodot[i]**2)
# vel_command.linear.y = yrobodot[i]
vel_command.linear.z = 0
vel_command.angular.x = 0
vel_command.angular.y = 0
vel_command.angular.z = wrobo[i]
self.vel_publisher.publish(vel_command)
r2.sleep()
path_index = 5;
path = Path()
path.header.frame_id ='map'
path.poses = [PoseStamped]*int((self.tf-self.t0)/delt+1)
s=0
for k in range(0,int((perturbation_at_time-self.t0)/delt)+1):
temp = PoseStamped()
# temp.position.x = xrobo1[k,j];
# temp.position.y = yrobo1[k,j];
temp.header.frame_id ='map'
temp.pose.position.x = xrobo[k];
temp.pose.position.y = yrobo[k];
temp.pose.position.z = 0.1
path.poses[k] = temp
s=s+1
for k in range(0,i_pert):
# print 'hi'
temp = PoseStamped()
# temp.position.x = xrobo1[k,j];
# temp.position.y = yrobo1[k,j];
temp.header.frame_id ='map'
temp.pose.position.x = xrobo1[k,path_index];
temp.pose.position.y = yrobo1[k,path_index];
temp.pose.position.z = 0.1
path.poses[k+s] = temp
self.paths_publisher[path_index].publish(path)
for i in range (0,i_pert):
vel_command = Twist()
vel_command.linear.x = math.sqrt(xrobodot1[i,path_index]**2+yrobodot1[i,path_index]**2)
# vel_command.linear.y = yrobodot[i]
vel_command.linear.z = 0
vel_command.angular.x = 0
vel_command.angular.y = 0
vel_command.angular.z = wrobo1[i,path_index]
self.vel_publisher.publish(vel_command)
r2.sleep()
print rospy.Time.now().secs
def newPathFromAStar(path): i=15 #每１米最少一个目标点 防止长距离角度过小的碰撞 poses=[] length=len(path.poses) if length>15: lastj=0 lastGD=quat_to_angle(path.poses[15].pose.orientation) poses.append(path.poses[15]) while (i<length-20) and (length>15): GD=quat_to_angle(path.poses[i].pose.orientation) errDirection=GD-lastGD if(errDirection>3.14): errDirection=2*3.1415-errDirection elif(errDirection<-3.14): errDirection=2*3.1415+errDirection #0.175 10du 0.35 20 0.524 30degree #遇到拐角的地方 向外进行扩展目标点 根据斜率进行扩展 if(abs(errDirection))>0.35: #向外部扩展目标点 x=path.poses[i].pose.position.x y=path.poses[i].pose.position.y x1=path.poses[i+10].pose.position.x y1=path.poses[i+10].pose.position.y x2=path.poses[i-10].pose.position.x y2=path.poses[i-10].pose.position.y k1=Slope(x1,x,y1,y) k2=Slope(x,x2,y,y2) if y1>y2: if x1>x2: if k1>k2: path.poses[i].pose.position.x=x1 path.poses[i].pose.position.y=y2 else: path.poses[i].pose.position.x=x2 path.poses[i].pose.position.y=y1 else: if k1>k2: path.poses[i].pose.position.x=x2 path.poses[i].pose.position.y=y1 else: path.poses[i].pose.position.x=x1 path.poses[i].pose.position.y=y2 else: if x1<x2: if k1>k2: path.poses[i].pose.position.x=x1 path.poses[i].pose.position.y=y2 else: path.poses[i].pose.position.x=x2 path.poses[i].pose.position.y=y1 else: if k1>k2: path.poses[i].pose.position.x=x2 path.poses[i].pose.position.y=y1 else: path.poses[i].pose.position.x=x1 path.poses[i].pose.position.y=y2 poses.append(path.poses[i]) lastGD=GD lastj=i if(i-lastj>50): lastj=i poses.append(path.poses[i]) i+=10 poses.append(path.poses[len(path.poses)-1]) mPath=Path() mPath.header.frame_id=path.header.frame_id mPath.poses=poses[:] return mPath
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)