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 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 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 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 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 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 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 Visualize_Path(path): #Function to publish path
print "visualizing path"
Path = rospy.Publisher('/Path', GridCells)
Path_GridCells = GridCells()
Path_GridCells.header = HEADER
Path_GridCells.cell_width = RESOLUTION
Path_GridCells.cell_height = RESOLUTION
Path_GridCells.cells = path
Path.publish(Path_GridCells)
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 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 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 readPathFromFile(filename):
try:
# read file
with open(filename) as f:
pathString = f.read()
# deserialize it into PoseArray
path = Path()
print "about to deserialize"
path.deserialize(pathString)
print "deserialized"
return path
except:
print "Returning no path for " + filename
return None
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 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 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 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 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 reconstructPath(checked, xInit, yInit, xEnd, yEnd):
Path = []
currentCell = (xEnd, yEnd)
# while the path is not back home yet
while currentCell[0] != xInit or currentCell[1] != yInit:
# set x and y values for current cell
curr_x = currentCell[0]
curr_y = currentCell[1]
# print "path x", curr_x , "path y" , curr_y
# get location of current cell in list
currentIndex = getIndexPlace(checked, curr_x, curr_y)
# set the next cell to be the came from cell of the current cell
nextCell = checked[currentIndex].cameFrom
# add the current cell to the list
Path.append(currentCell)
# change the current cell to be the next cell
currentCell = nextCell
initCell = (xInit, yInit)
Path.append(initCell)
# print "Path", Path
return Path
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 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 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
for pose in msg.poses:
pose_stamped = PoseStamped()
pose_stamped.header = msg.header
pose_stamped.pose = pose
path_sys_ball_pred.poses.append(pose_stamped)
publish_path_sys_ball_prediction.publish(path_sys_ball_pred)
#MAIN
if __name__ == '__main__':
rospy.init_node('sv_path', anonymous=True)
rospy.sleep(1)
path_sim_ball = Path()
path_sys_ball = Path()
path_sys_ball_fil = Path()
initialize()
rospy.Subscriber("/sv_simulation/tt_ball_position",
PoseStamped,
pather_sim_ball,
queue_size=1)
rospy.Subscriber("/sv_control_system/control_system/ball_position",
PoseStamped,
pather_sys_ball,
queue_size=1)
rospy.Subscriber(
"/sv_control_system/control_system/ball_position_filtered",
PoseStamped,
rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped,
self.callback1)
if abs(self.pose_x - path.poses[self.point_step].pose.
position.x) < 0.2 and abs(self.pose_y - path.poses[
self.point_step].pose.position.y) < 0.2:
self.point_step = self.point_step + 1
self.pub = True
print self.point_step
if self.point_step > len(path_a) - 1:
self.point_step = 0
path.poses = []
except rospy.ROSInterruptException:
pass
rate.sleep()
if __name__ == '__main__':
try:
path = Path()
pose = PoseStamped()
nav_goal = PoseStamped()
Run = client()
Run.main1()
except rospy.ROSInterruptException:
pass
def handle_multi_planner(req):
_map = load_map(
os.path.realpath(os.path.join(__file__, "../../../")) +
'/multi_robot_action_move/map/' + sys.argv[1])
map_resolution = 1
# _map = rospy.wait_for_message("/map",OccupancyGrid) # change the topic based on the namespace
# map_resolution = round(_map.info.resolution,2)
# map_width = _map.info.width
# map_height = _map.info.height
# map = np.array(_map.data)
# map = np.reshape(map,(map_height,map_width))
agent_pos = []
_agent = 0
for pos in req.start.robot_name_pose:
agent_pos.append(
(round(pos.robot_pose.pose.position.x / map_resolution,
0), round(pos.robot_pose.pose.position.y / map_resolution,
0))) #round based on map resolution
it_jobs = [round(elem, 2) for elem in req.jobs]
print(it_jobs)
list_jobs = [it_jobs[i:i + 5] for i in range(0, len(it_jobs), 5)]
print(list_jobs)
jobs = []
for sublist in list_jobs:
tuple_start = (round(sublist[0] / map_resolution,
0), round(sublist[1] / map_resolution, 0))
tuple_goal = (round(sublist[2] / map_resolution,
0), round(sublist[3] / map_resolution, 0))
job = (tuple_start, tuple_goal, sublist[4])
print(job)
jobs.append(job)
grid = np.repeat(_map[:, ::2, np.newaxis], 100, axis=2)
# grid = np.repeat(map[:, ::, np.newaxis], 100, axis=2)
config = generate_config()
config['filename_pathsave'] = req.fname
config['finished_agents_block'] = True
print("Problem:")
print(str(jobs))
print(str(agent_pos))
print("GREEDY")
gui_path_array = Path_array_agents_id()
res_agent_job, tuple_paths = plan_greedy(agent_pos, jobs, grid, config)
max_job_size = max([len(i) for i in res_agent_job]) * 2
gui_path_array.max_job_size = max_job_size
for robo in res_agent_job:
agent_robo_job = agent_job()
for jobs in robo:
agent_robo_job.agent_robo_job.append(jobs)
gui_path_array.agent_job.append(agent_robo_job)
print("tuple_paths")
print(tuple_paths)
for agent in tuple_paths:
_seq = 0
_agent_paths = agent_paths_id()
for job in agent:
gui_path = Path()
Poses = []
for pos in job:
pose = PoseStamped()
pose.header.seq = _seq
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = pos[0] * map_resolution
pose.pose.position.y = pos[1] * map_resolution
pose.pose.position.z = 0.0
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 1.0
Poses.append(pose)
_seq = _seq + 1
gui_path.header.frame_id = "map"
gui_path.header.stamp = rospy.Time.now()
gui_path.poses = Poses
if len(gui_path.poses) == 1:
_agent_paths.agent_paths.append(gui_path)
elif gui_path.poses[0].pose != gui_path.poses[-1].pose:
_agent_paths.agent_paths.append(gui_path)
_agent_paths.robot_name = req.start.robot_name_pose[_agent].robot_name
_agent = _agent + 1
gui_path_array.path_array.append(_agent_paths)
_agent = 0
return gui_path_array
#Node and msg initialization
rospy.init_node('path_ekf_plotter')
#Rosparams that are set in the launch
#max size of array pose msg from the path
if not rospy.has_param("~max_list_append"):
rospy.logwarn('The parameter max_list_append dont exists')
max_append = rospy.set_param("~max_list_append", 1000)
max_append = 1000
if not (max_append > 0):
rospy.logwarn('The parameter max_list_append not is correct')
sys.exit()
pub = rospy.Publisher('/ekfpath', Path, queue_size=1)
path = Path() #creamos el mensaje path de tipo path
msg = Odometry()
#Subscription to the topic
msg = rospy.Subscriber('/robot_pose_ekf/odom_combined',
PoseWithCovarianceStamped, callback)
rate = rospy.Rate(30) # 30hz
try:
while not rospy.is_shutdown():
#rospy.spin()
rate.sleep()
except rospy.ROSInterruptException:
pass
from geometry_msgs.msg import PoseStamped
import math
import rospy
import tf.transformations as tf
topic = 'test_path'
publisher = rospy.Publisher(topic, Path, queue_size=1)
rospy.init_node('send_path')
t = 0
rate = rospy.Rate(30)
timestep = rate.sleep_dur.to_sec()
while not rospy.is_shutdown():
p = Path()
p.header.frame_id = "base_link"
p.header.stamp = rospy.Time.now()
num_points = 50
for i in range(0, num_points):
ps = PoseStamped()
ps.header.stamp = p.header.stamp
ps.header.frame_id = p.header.frame_id
ps.pose.position.x = 10.0 * i / (num_points - 1) - 5
ps.pose.position.y = math.sin(10.0 * i / num_points + t)
ps.pose.position.z = 0
angle = 2 * math.pi * i / num_points + t
quat = tf.quaternion_from_euler(angle, 0, 0)
ps.pose.orientation.x = quat[0]
ps.pose.orientation.y = quat[1]
def estimate_object_pose(self, image_msg):
"""
Receive an image, pass it through the network, and get the
estimated gate translation and rotation in camera frame.
Args:
image_msg: RGB image
Returns:
The detected translation as a 3-vector (np.array, [x,y,z]) and
orientation as a quaternion 4-vector(np.array, [x,y,z,w])
"""
# update MonoWaypointDetector
self.mode = rospy.get_param("riseq/monocular_cv", 'disable')
if(self.mode != 'disable'):
img = self.bridge.imgmsg_to_cv2(image_msg, "rgb8")
path = Path()
path.header.stamp = rospy.Time.now()
path.header.frame_id = ""
wp = PoseStamped()
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = ""
ladder_info = RiseSaccHelix()
ladder_info.header.stamp = rospy.Time.now()
ladder_info.header.frame_id = ""
ladder_info.width = img.shape[1]
ladder_info.height = img.shape[0]
if(self.mode == 'window'):
R, t, R_exp, cnt = self.wd.detect(img.copy(), self.max_size)
if cnt is not None:
img = cv2.drawContours(img, [cnt[1:]], -1, (255,0,0), 3)
img = self.wd.draw_frame(img, (cnt[0][0],cnt[0][1]), R_exp, t)
R = np.concatenate((R, np.array([[0.0, 0.0, 0.0]])), axis = 0)
R = np.concatenate((R, np.array([[0.0, 0.0, 0.0, 1.0]]).T ), axis = 1)
gate_quat = tf.transformations.quaternion_from_matrix(R)
# gate waypoint
wp.pose.position.x = t[2][0]
wp.pose.position.y = -t[0][0]
wp.pose.position.z = t[1][0]
wp.pose.orientation.x = gate_quat[0]
wp.pose.orientation.y = gate_quat[1]
wp.pose.orientation.z = gate_quat[2]
wp.pose.orientation.w = gate_quat[3]
path.poses = [wp]
elif(self.mode == 'pipe'):
ellipses = self.pd.detect(img.copy(), self.max_size)
if len(ellipses) == 1:
R, t, R_exp, e = ellipses[0]
img = cv2.ellipse(img, e, (255,0,0), 2)
img = self.pd.draw_frame(img, (e[0][0],e[0][1]), R_exp, t)
R = np.concatenate((R, np.array([[0.0, 0.0, 0.0]])), axis = 0)
R = np.concatenate((R, np.array([[0.0, 0.0, 0.0, 1.0]]).T ), axis = 1)
gate_quat = tf.transformations.quaternion_from_matrix(R)
# gate waypoint
wp.pose.position.x = t[2][0]
wp.pose.position.y = -t[0][0]
wp.pose.position.z = t[1][0]
wp.pose.orientation.x = gate_quat[0]
wp.pose.orientation.y = gate_quat[1]
wp.pose.orientation.z = gate_quat[2]
wp.pose.orientation.w = gate_quat[3]
path.poses = [wp]
elif(self.mode == 'gate'):
R, t, img, conf = self.gateDetector.predict(img.copy())
gate_x = rospy.get_param("riseq/gate_x", 0.0)
gate_y = rospy.get_param("riseq/gate_y", 0.0)
gate_z = rospy.get_param("riseq/gate_z", 0.0)
gate_roll = rospy.get_param("riseq/gate_roll", 0.0)
gate_pitch = rospy.get_param("riseq/gate_pitch", 0.0)
gate_yaw = rospy.get_param("riseq/gate_yaw", 0.0)
gate_quat = tf.transformations.quaternion_from_euler(0.0, gate_pitch, gate_roll, axes='rzyx')
# gate waypoint
wp.pose.position.x = gate_x
wp.pose.position.y = gate_y
wp.pose.position.z = gate_z
wp.pose.orientation.x = gate_quat[0]
wp.pose.orientation.y = gate_quat[1]
wp.pose.orientation.z = gate_quat[2]
wp.pose.orientation.w = gate_quat[3]
elif(self.mode == 'ladder'):
bbox, outimg = self.ld.detect(img.copy(), self.max_size)
if bbox is not None:
x,y,w,h = bbox
print("Bounding box:",x,y,w,h)
ladder_info.depth = 1.5 + np.random.rand()*0.25
ladder_info.x = int(x + w/2)
ladder_info.y = int(y + h/2)
img = cv2.rectangle(img,(x,y),(x+w,y+h),(0,255,0),2)
img = cv2.circle(img, (int(x + w/2),(y + h/2)), 4, (0,255,0), -1)
else:
img = outimg
else:
rospy.loginfo("Monocular Object Detector mode non-existent.")
#self.waypoint_pub.publish(path)
self.ladder_info_pub.publish(ladder_info)
img = self.bridge.cv2_to_imgmsg(img, "rgb8")
self.img_dect_pub.publish(img)
else:
# Do nothing
print("Monocular mode: {}".format(self.mode))
pass
def interpolation_publish_ENU(self):
"""Interpolates between all ENU coordinates and publishes them as a Path.
Subscribes
----------
None
Publishes
---------
Topic: /planning/trajectory
Msg: Path
"""
path_publisher = rospy.Publisher('/planning/trajectory',
Path,
queue_size=1000)
rate = rospy.Rate(1)
eData, nData, uData = super(PathInterpolatorENU,
self).geodetic_data_to_ENU_data()
# Contains lists of fine points, including coarse points
eInterpolatedPositions = []
nInterpolatedPositions = []
uInterpolatedPositions = []
numberOfCoarsePoints = len(eData)
for i in range(numberOfCoarsePoints):
finePointsE, finePointsN, finePointsU = self.interpolate_ENU(
i, eData, nData, uData, numberOfCoarsePoints)
eInterpolatedPositions.extend(finePointsE)
nInterpolatedPositions.extend(finePointsN)
uInterpolatedPositions.extend(finePointsU)
while not rospy.is_shutdown():
path = Path()
for i in range(len(nInterpolatedPositions)):
# # Attempting to add points directly in one line without creating point object first
# path.poses.append(path.poses[i].pose.position.x = 0.0) # finePointsE[i]
# path.poses[i].pose.position.y = 0.0 # finePointsN[i]
# path.poses[i].pose.position.z = 0.0
currentPoseStampMsg = PoseStamped()
h = Header()
h.stamp = rospy.Time.now()
h.seq = i
currentPoseStampMsg.header.seq = h.seq
currentPoseStampMsg.header.stamp = h.stamp
currentPoseStampMsg.pose.position.x = eInterpolatedPositions[i]
currentPoseStampMsg.pose.position.y = nInterpolatedPositions[i]
currentPoseStampMsg.pose.position.z = uInterpolatedPositions[i]
path.poses.append(currentPoseStampMsg)
path_publisher.publish(path)
rospy.loginfo("Published Path with %d steps", i + 1)
rate.sleep()
def selectTarget(self):
# IMPORTANT: The robot must be stopped if you call this function until
# it is over
# Check if we have a map
while self.robot_perception.have_map == False:
Print.art_print("Navigation: No map yet", Print.RED)
return
print "\nClearing all markers"
RvizHandler.printMarker(\
[[0, 0]],\
1, # Type: Arrow
3, # Action: delete all
"map", # Frame
"null", # Namespace
[0,0,0,0], # Color RGBA
0.1 # Scale
)
print '\n\n----------------------------------------------------------'
print "Navigation: Producing new target"
# We are good to continue the exploration
# Make this true in order not to call it again from the speeds assignment
self.target_exists = True
# Gets copies of the map and coverage
local_ogm = self.robot_perception.getMap()
local_ros_ogm = self.robot_perception.getRosMap()
local_coverage = self.robot_perception.getCoverage()
print "Got the map and Coverage"
self.path_planning.setMap(local_ros_ogm)
# Once the target has been found, find the path to it
# Get the global robot pose
g_robot_pose = self.robot_perception.getGlobalCoordinates(\
[self.robot_perception.robot_pose['x_px'],\
self.robot_perception.robot_pose['y_px']])
# Call the target selection function to select the next best goal
# Choose target function
self.path = []
force_random = False
while len(self.path) == 0:
start = time.time()
target = self.target_selection.selectTarget(\
local_ogm,\
local_coverage,\
self.robot_perception.robot_pose,
self.robot_perception.origin,
self.robot_perception.resolution,
force_random)
self.path = self.path_planning.createPath(\
g_robot_pose,\
target,
self.robot_perception.resolution)
print "Navigation: Path for target found with " + str(len(self.path)) +\
" points"
if len(self.path) == 0:
Print.art_print(\
"Path planning failed. Fallback to random target selection", \
Print.RED)
force_random = True
# Reverse the path to start from the robot
self.path = self.path[::-1]
#########################################
# Extra challenge #1
# A. Smooth path planner
if len(self.path) > 3:
x = np.array(self.path)
y = np.copy(x)
a = 0.5
b = 0.1
# FORMULA : y_i = y_i + a * (x_i - y_i) + b * (y_i+1 - 2 * y_i + y_i+1)
epsilon = np.sum(np.abs(a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :])))
while epsilon > 1e-3:
y[1:-1, :] += a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :])
epsilon = np.sum(np.abs(a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :])))
# Copy the smoother path
self.path = y.tolist()
# Break the path to subgoals every 2 pixels (1m = 20px)
step = 1
n_subgoals = (int) (len(self.path)/step)
self.subtargets = []
for i in range(0, n_subgoals):
self.subtargets.append(self.path[i * step])
self.subtargets.append(self.path[-1])
self.next_subtarget = 0
print "The path produced " + str(len(self.subtargets)) + " subgoals"
######################### NOTE: QUESTION ##############################
# The path is produced by an A* algorithm. This means that it is
# optimal in length but 1) not smooth and 2) length optimality
# may not be desired for coverage-based exploration
########################################################################
self.counter_to_next_sub = self.count_limit
# Publish the path for visualization purposes
ros_path = Path()
ros_path.header.frame_id = "map"
for p in self.path:
ps = PoseStamped()
ps.header.frame_id = "map"
ps.pose.position.x = 0
ps.pose.position.y = 0
######################### NOTE: QUESTION ##############################
# Fill the ps.pose.position values to show the path in RViz
# You must understand what self.robot_perception.resolution
# and self.robot_perception.origin are.
ps.pose.position.x = p[0] * self.robot_perception.resolution + self.robot_perception.origin['x']
ps.pose.position.y = p[1] * self.robot_perception.resolution + self.robot_perception.origin['y']
########################################################################
ros_path.poses.append(ps)
self.path_publisher.publish(ros_path)
# Publish the subtargets for visualization purposes
subtargets_mark = []
for s in self.subtargets:
subt = [
s[0] * self.robot_perception.resolution + \
self.robot_perception.origin['x'],
s[1] * self.robot_perception.resolution + \
self.robot_perception.origin['y']
]
subtargets_mark.append(subt)
RvizHandler.printMarker(\
subtargets_mark,\
2, # Type: Sphere
0, # Action: Add
"map", # Frame
"art_subtargets", # Namespace
[0, 0.8, 0.0, 0.8], # Color RGBA
0.2 # Scale
)
self.inner_target_exists = True
def img_callback(msg):
start = time.time()
img = CvBridge().imgmsg_to_cv2(msg, "bgr8").copy()
seq = msg.header.seq
try:
#cv2.imwrite("inputs/input_{}.png".format(str(msg.header.seq)), img)
detector = LaneDetection([img.copy(), seq])
pre_process = PreProcessImg([img.copy(), seq])
# Pre Process
pre_success, processedImg = pre_process.process_image()
if pre_success == False:
print("\n|| COULD NOT FOUND RED LANE FROM PREPROCESSING IMAGE ||\n")
return False
# Lane Detection
blended_img, out_img, lane_found, path = detector.process_image(processedImg)
########################################### output contatenation
out_img = cv2.copyMakeBorder(
out_img,
(img.shape[0] - out_img.shape[0])/2,
(img.shape[0] - out_img.shape[0])/2,
(img.shape[1] - out_img.shape[1])/2,
(img.shape[1] - out_img.shape[1])/2,
cv2.BORDER_CONSTANT,
value=(128,128,128)
)
comb = np.concatenate((blended_img, out_img), axis=0)
########################################### output contatenation
if lane_found:
pose_list = []
global past_coords
past_coords.append([path[1][0], path[1][1], path[0][2]])
for i in range(len(path)):
pose = PoseStamped()
pose.pose.position.x = path[i][0]
pose.pose.position.y = path[i][1]
pose.pose.position.z = path[i][2]
quat = tf.transformations.quaternion_from_euler(0,0, np.deg2rad(path[i][2]))
pose.pose.orientation.x = quat[0]
pose.pose.orientation.y = quat[1]
pose.pose.orientation.z = quat[2]
pose.pose.orientation.w = quat[3]
pose_list.append(pose)
path_msg = Path()
path_msg.poses = pose_list
path_pub.publish(path_msg)
org1 = (10, comb.shape[1]+35)
org2 = (10, comb.shape[1]+55)
org3 = (10, comb.shape[1]+75)
org4 = (10, comb.shape[1]+15)
# Using cv2.putText() method
cv2.putText(comb, "x:{0}, y:{1}, yaw:{2}".format(path[0][0], path[0][1], round(path[0][2], 1)), org1, cv2.FONT_HERSHEY_SIMPLEX,
0.5, (0, 0, 255), 1, cv2.LINE_AA)
cv2.putText(comb, "x:{0}, y:{1}, yaw:{2}".format(path[1][0], path[1][1], round(path[1][2], 1)), org2, cv2.FONT_HERSHEY_SIMPLEX,
0.5, (0, 255, 0), 1, cv2.LINE_AA)
cv2.putText(comb, "x:{0}, y:{1}, yaw:{2}".format(path[2][0], path[2][1], round(path[2][2], 1)), org3, cv2.FONT_HERSHEY_SIMPLEX,
0.5, (255, 0, 0), 1, cv2.LINE_AA)
cv2.putText(comb, "Lane Found: {}".format(lane_found), org4, cv2.FONT_HERSHEY_SIMPLEX,
0.5, (120, 180, 75), 1, cv2.LINE_AA)
else:
past_coords = []
cv2.putText(comb, "LANE IS NOT FOUND !!!".format(lane_found), (10, comb.shape[1]+15), cv2.FONT_HERSHEY_SIMPLEX,
0.5, (120, 180, 75), 1, cv2.LINE_AA)
cv2.imshow("Lane Detection", comb)
cv2.waitKey(1)
img_pub.publish(CvBridge().cv2_to_imgmsg(comb, "bgr8"))
pr_img_pub.publish(CvBridge().cv2_to_imgmsg(processedImg))
blended_img_pub.publish(CvBridge().cv2_to_imgmsg(blended_img, "bgr8"))
tf_img_pub.publish(CvBridge().cv2_to_imgmsg(out_img, "bgr8"))
#cv2.imwrite("outputs/output_{}.png".format(str(msg.header.seq)), comb)
except Exception as e:
print("ERROR!")
print(traceback.format_exc())
img_pub.publish(CvBridge().cv2_to_imgmsg(img.copy(), "bgr8"))
#quit()
return False
t = time.time() - start
f = 1/t
print("time : {0}, frequency: {1}".format(t, f))
def __init__(self):
self.lock = RLock()
# Initialise robot
self.time = None # variable to keep current time
self.seq = 0
# Initialise a robot pose and covariance
robot_pose = np.array([[0.], [0.], [0.]])
robot_pose = np.reshape(robot_pose, (3, 1))
robot_covariance = np.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]
]) * 10**(-3)
sense_Type = 'Vision'
self.robot = Robot(robot_pose, robot_covariance, sense_Type)
# Initialise motion
self.motion_command = np.array([[0.], [0.], [0.]])
self.motion_covariance = np.array([[1., 0., 0.], [0., 1., 0.],
[0., 0.,
1.]]) # Just initialization
# Initialise landmark measurement
self.measurement_covariance = np.array([[1.0, 0.0], [0.0, 1.0]
]) * 10**(-3)
# Initialise kalman filter
# Initialise a state mean and covariance
state_mean = robot_pose
state_covariance = robot_covariance
# initial state contains initial robot pose and covariance
self.KF = KalmanFilter(state_mean, state_covariance, self.robot)
# Subscribe to the cylinder node output
rospy.Subscriber('/landmarks',
Landmarks_Msg,
self.callbackLandmarkMeasurement,
queue_size=1,
buff_size=2)
# Subscribe to odometry
rospy.Subscriber('/odom', Odometry, self.callbackOdometryMotion)
# Initialise a state and covariance publisher
self.pub_odom = rospy.Publisher('/robot_slam_odometry',
Odometry,
queue_size=5)
self.pub_traj = rospy.Publisher('/robot_trajectory',
Path,
queue_size=5)
self.pub_landmarks = rospy.Publisher('/landmarks_state',
MarkerArray,
queue_size=5)
self.pub_det = rospy.Publisher('/det', Float32, queue_size=5)
# Initialise the trajectory message
self.pose_count = 0
self.traj_msg = Path()
self.traj_msg.header.frame_id = "odom"
r = rospy.Rate(10)
while not rospy.is_shutdown():
self.publishState()
self.publish_traj()
r.sleep()
# Start streaming
prof_tr = pipeline_trajectory.start(config_trajectory)
# Align to depth
align_to = rs.stream.color
align = rs.align(align_to)
# Node init and publisher definition
rospy.init_node('odometry', anonymous=True)
pub_path = rospy.Publisher("path", Path, queue_size=100)
pub_odom = rospy.Publisher('odom_t265', Odometry, queue_size=5)
rate = rospy.Rate(200) # 30hz
# init trajectory variables
my_path = Path()
my_path.header.frame_id = 'central'
# Processing blocks
pc = rs.pointcloud()
decimate = rs.decimation_filter()
decimate.set_option(rs.option.filter_magnitude, 2**1)
colorizer = rs.colorizer()
old_points = np.array([[0, 0, 0]])
#print("Start node")
rospy.loginfo("odometry is running")
while not rospy.is_shutdown():
# Get data from cameras
trajectory = pipeline_trajectory.wait_for_frames()
def __init__(self, beam_num, index, num_env):
self.index = index
self.num_env = num_env
self.robot_radius = [0.3]
node_name = 'Gazebo_Env_' + str(index)
rospy.init_node(node_name, anonymous=None)
self.beam_mum = beam_num
self.laser_cb_num = 0
self.scan = None
# used in reset_world
self.self_speed = [0.0, 0.0]
self.step_goal = [0., 0.]
self.step_r_cnt = 0.
# used in generate goal point
self.map_size = np.array([8., 8.], dtype=np.float32) # 20x20m
self.goal_size = 0.5
self.robot_value = 10.
self.goal_value = 0.
# -----------Publisher and Subscriber-------------
#cmd_vel_topic = 'robot_' + str(index) + '/cmd_vel'
cmd_vel_topic = '/cmd_vel'
self.cmd_vel = rospy.Publisher(cmd_vel_topic, Twist, queue_size=10)
#object_state_topic = 'robot_' + str(index) + '/base_pose_ground_truth'
#object_state_topic = '/amcl_pose'
#object_state_topic = '/odom'
#self.object_state_sub = rospy.Subscriber(object_state_topic, Odometry, self.ground_truth_callback)
#amcl_pose_topic = '/amcl_pose'
#self.amcl_pose_sub = rospy.Subscriber(amcl_pose_topic, PoseWithCovarianceStamped, self.amcl_pose_callback)
#laser_topic = 'robot_' + str(index) + '/base_scan'
laser_topic = '/scan'
self.laser_sub = rospy.Subscriber(laser_topic, LaserScan,
self.laser_scan_callback)
odom_topic = '/odom'
self.odom_sub = rospy.Subscriber(odom_topic, Odometry,
self.odometry_callback)
self.sim_clock = rospy.Subscriber('clock', Clock,
self.sim_clock_callback)
self.set_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
# -----------rviz----------------------
#ininit_amcl_topic = 'initialpose'
#self.pub_amcl_init = rospy.Publisher(ininit_amcl_topic, PoseWithCovarianceStamped, queue_size=10)
# -----------Service-------------------
#self.reset_stage = rospy.ServiceProxy('reset_positions', Empty)
self.reset_gazebo = rospy.ServiceProxy('/gazebo/reset_simulation',
Empty)
self.is_sub_goal = False
self.tf_listener = tf.TransformListener()
self.PathMsg = Path()
# # Wait until the first callback
self.speed = None
self.state = None
self.speed_GT = None
self.state_GT = None
self.is_crashed = None
self.is_collision = 0
self.lidar_danger = 0.5
self.scan_min = 6.0
self.init_pose = [8, 0, 3.14]
self.goal_point = [0, 0]
self.pre_distance = 0
self.distance = 0
self.frist_distance = 16
#self.goal_model = Respawn(self.goal_point[0], self.goal_point[1], 'goal')
while self.scan is None or self.speed is None or self.state is None or self.speed_GT is None or self.state_GT is None:
pass
print("env")
rospy.sleep(1.)
def test_interpolate_path_rotation_left(self):
""" Test interpolation for rotation to the left
Accelerate at 1 rad/s2 to 1rad/s to the left, keep 1s and deccelerate to standing still
Total movement should be 2 rad.
Trajectory is tested at different times
"""
self._target_x_vel = 1.0
self._target_x_acc = 1.0
self._target_yaw_vel = 1.0
self._target_yaw_acc = 1.0
path = Path()
pose1 = PoseStamped()
quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0)
pose1.pose.orientation.x = quaternion[0]
pose1.pose.orientation.y = quaternion[1]
pose1.pose.orientation.z = quaternion[2]
pose1.pose.orientation.x = quaternion[3]
path.poses.append(pose1)
pose2 = PoseStamped()
pose2.pose.position.x = 0.0
quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, 2.0)
pose2.pose.orientation.x = quaternion[0]
pose2.pose.orientation.y = quaternion[1]
pose2.pose.orientation.z = quaternion[2]
pose2.pose.orientation.w = quaternion[3]
path.poses.append(pose2)
current_section = SectionInterpolation(pose1, pose2, rospy.Time(0.0),
self._target_x_vel, self._target_x_acc,
self._target_yaw_vel, self._target_yaw_acc)
# Acceleration phase x/theta = 0.5*a*t^2, v/dtheta=a*t
tp = current_section.interpolate_with_acceleration(rospy.Time(0.0))
yaw = self.quaternion_to_yaw(tp.pose.pose.orientation)
self.assertEqual(yaw, 0.0)
self.assertEqual(tp.velocity.angular.z, 0.0)
tp = current_section.interpolate_with_acceleration(rospy.Time(0.5))
yaw = self.quaternion_to_yaw(tp.pose.pose.orientation)
self.assertAlmostEqual(yaw, 0.125)
self.assertAlmostEqual(tp.velocity.angular.z, 0.5)
tp = current_section.interpolate_with_acceleration(rospy.Time(1.0))
yaw = self.quaternion_to_yaw(tp.pose.pose.orientation)
self.assertAlmostEqual(yaw, 0.5)
self.assertAlmostEqual(tp.velocity.angular.z, 1.0)
# Constant velocity phase x/theta = 0.5 + a*t, v/dtheta = 1.0
tp = current_section.interpolate_with_acceleration(rospy.Time(1.5))
yaw = self.quaternion_to_yaw(tp.pose.pose.orientation)
self.assertAlmostEqual(yaw, 1.0)
self.assertAlmostEqual(tp.velocity.angular.z, 1.0)
tp = current_section.interpolate_with_acceleration(rospy.Time(2.0))
yaw = self.quaternion_to_yaw(tp.pose.pose.orientation)
self.assertAlmostEqual(yaw, 1.5)
self.assertAlmostEqual(tp.velocity.angular.z, 1.0)
# Decceleration phase x/theta = 1.5 + 1.0*t - 0.5*a*t^2, v/dtheta=1.0-a*t
tp = current_section.interpolate_with_acceleration(rospy.Time(2.5))
yaw = self.quaternion_to_yaw(tp.pose.pose.orientation)
self.assertAlmostEqual(yaw, 1.875)
self.assertAlmostEqual(tp.velocity.angular.z, 0.5)
tp = current_section.interpolate_with_acceleration(rospy.Time(3.0))
yaw = self.quaternion_to_yaw(tp.pose.pose.orientation)
self.assertAlmostEqual(yaw, 2.0)
self.assertAlmostEqual(tp.velocity.angular.z, 0.0)
from math import * posePublisher=0 robotStatePublisher=0 seq=0 d1=0 a2=0 a3=0 a4=0 theta3=0 pathMsg = Path() def path_real_reset(data): global pathMsg pathMsg.poses = [] def msg_received(data): global posePublisher, robotStatePublisher, d1, a2, a3, a4, theta3, seq encoder0=data.encoder0_pos encoder1=data.encoder1_pos encoder2=data.encoder2_pos theta0 = (encoder0 / pow(2,11)) * pi
def __init__(self):
#Car state and limits - Run/ Stop ...
self.car_state = 0
# Rate for the final command
self.cmd_rate = 100
## Car positions and velocities
# Car position estimates w.r.t map (in m)
self.car_x = 0
self.car_y = 0 # assume initially its placed at 0 from wall
self.car_px = 0 # car previous positions
self.car_py = 0
self.car_yaw = 0
# Car position and orientation from amcl
self.car_ax = 0
self.car_ay = 0
self.car_ayaw = 0
# minimum distance from obstacles. This triggers an emergency stop.
self.dist_lim = 0.2
## Communication related
# For serial initializations
self.port = '/dev/ttyACM0'
self.baudrate = 115200 # bits/s
# Was serial successfully init flag
self.serial_flag = False
# Should serial be init flag
self.ser_initq = False
# for loop timing
self.current_time = 0
# Waypoint init
# Array to load waypoints
self.waypt_buf = np.zeros((2, 2))
# waypoint load rate
self.waypt_rate = 10 #(Hz)
# waypoint timing variables
self.waypt_ptime = 0 #The previous time when waypint was updated
# waypoint index
self.waypt_i = 0
# next desired x and y from waypt
self.waypt_xd = 0
self.waypt_yd = 0
# distance between present pos and desired pos
self.dis_err = 0
# limit for waypoint update
self.waypt_lim = 0.5 #2m limit, if dist between des and present is > waypt_lim then do not update
# Trajectory index
self.traj_idx = -1
self.running = False # flag for command publishing or not
# if something is too close
self.intrusion = False
# movebase
self.viaPts = Path()
self.pubVia = rospy.Publisher('via_points', Path, queue_size=1)
self.mode_pub = rospy.Publisher('/mobile_base_controller/control_mode',
Byte,
queue_size=1)
self.scan_min_ang = -1.
self.scan_max_ang = 1.
self.scan_ang_inc = 0.25 / 180 * np.pi
# barrier of the robot: intrusion within 30cm front of the laser center will induce an emergency shutdown.
self.shape = 0.3 / np.cos(
np.arange(self.scan_min_ang, self.scan_max_ang, self.scan_ang_inc))
# wolamy usluge okreslajac zadanie
resp = get_plan(start, goal, 1)
print resp
# zapisujemy sciezke do zmiennej globalnej
new_path = resp.plan
#print new_path
except rospy.ServiceException, e:
print "Service call failed: %s " % e
if __name__ == "__main__":
global new_path
global tfBuffer
global listener
global pose
new_path = Path()
new_vel = Twist()
rospy.init_node('zad3', anonymous=True)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
print("ready")
rate = rospy.Rate(10) # 10Hz
pub = rospy.Publisher('/mobile_base_controller/cmd_vel',
Twist,
queue_size=10)
get_path(2, 2)
while not rospy.is_shutdown():
# Wstawic kod odpowiedzialny za aktualizację zmiennej przechowujacej pozycje
# ...
if not len(new_path.poses) == 0:
# dostep do pozycji robota jest poprzez zmienna globalna 'pose'. Pozycja jest aktualizowana
def waypoints_callback(self, msg):
"""
Waypoints callback does way too much right now. All of the path stuff should be handled in a helper method
"""
google_points = []
#Reads each point in the waypoint topic into google_points
for gps_point in msg.waypoints:
point = gps_util.get_point(gps_point)
google_points.append(point)
print len(google_points)
#Adds more points between the google points
google_points_plus = gps_util.add_intermediate_points(
google_points, 15.0)
print len(google_points_plus)
ax = []
ay = []
extra_points = Path()
extra_points.header = Header()
extra_points.header.frame_id = '/map'
#Puts the x's and y's
for p in google_points_plus:
extra_points.poses.append(self.create_poseStamped(p))
ax.append(p.x)
ay.append(p.y)
self.points_pub.publish(extra_points)
#calculate the spline
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=0.1)
path = Path()
path.header = Header()
path.header.frame_id = '/map'
for i in range(0, len(cx)):
curve_point = Point()
curve_point.x = cx[i]
curve_point.y = cy[i]
path.poses.append(self.create_poseStamped(curve_point))
self.path_pub.publish(path)
#================================================ pure persuit copy/pase ===============================================
k = 0.1 # look forward gain
Lfc = 3.5 # look-ahead distance
Kp = 1.0 # speed propotional gain
dt = 0.1 # [s]
L = 2.9 # [m] wheel base of vehicle
target_speed = 10.0 / 3.6 # [m/s]
T = 100.0 # max simulation time
# initial state
pose = self.odom.pose.pose
twist = self.odom.twist.twist
quat = (pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w)
angles = tf.euler_from_quaternion(quat)
initial_v = math.sqrt(twist.linear.x**2 + twist.linear.y**2)
state = State(x=pose.position.x,
y=pose.position.y,
yaw=angles[2],
v=initial_v) #TODO this has to be where we start
lastIndex = len(cx) - 1
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = pure_pursuit.calc_target_index(state, cx, cy)
while lastIndex > target_ind:
ai = pure_pursuit.PIDControl(target_speed, state.v)
di, target_ind = pure_pursuit.pure_pursuit_control(
state, cx, cy, target_ind)
#publish where we want to be
mkr = self.create_marker(cx[target_ind], cy[target_ind], '/map')
self.target_pub.publish(mkr)
#publish an arrow with our twist
arrow = self.create_marker(0, 0, '/base_link')
arrow.type = 0 #arrow
arrow.scale.x = 2.0
arrow.scale.y = 1.0
arrow.scale.z = 1.0
arrow.color.r = 1.0
arrow.color.g = 0.0
arrow.color.b = 0.0
#TODO di might be in radians so that might be causing the error
quater = tf.quaternion_from_euler(0, 0, di)
arrow.pose.orientation.x = quater[0]
arrow.pose.orientation.y = quater[1]
arrow.pose.orientation.z = quater[2]
arrow.pose.orientation.w = quater[3]
self.target_twist_pub.publish(arrow)
#go back to pure persuit
state = self.update(state, ai, di)
#time = time + dt
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
# Test
assert lastIndex >= target_ind, "Cannot goal"
rospy.logerr("Done navigating")
msg = VelAngle()
msg.vel = 0
msg.angle = 0
msg.vel_curr = 0
self.motion_pub.publish(msg)
# 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()
#!/usr/bin/env python
from std_msgs.msg import Float32
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseArray, Pose, PoseStamped
import rospy
import roslib
import tf
import numpy as np
import matplotlib.pyplot as plt
import time
show_animation = False
global_path = Path()
path_received = False
lookahead_distance = 8
if __name__ == '__main__':
text_file = open("road.txt", "a")
rospy.init_node('carlaodom')
listener = tf.TransformListener()
prev_trans = [0, 0]
string = ''
while not rospy.is_shutdown():
next_point = pathpoints[next_idx]
alpha = math.atan2(next_point[1] - current_point[1],
next_point[0] - current_point[0])
theta = euler[2] - alpha
dx = x - current_point[0]
dy = y - current_point[1]
offset = -dx*math.sin(alpha) + dy*math.cos(alpha)
offset_pub.publish(offset + L*np.sin(theta))
### DEBUG ###
path = Path()
path.header.frame_id = 'map'
path.poses = []
for i in range(5):
idx = (current_idx + i) % len(pathpoints)
pnt = pathpoints[idx]
pose = PoseStamped()
pose.header.frame_id = 'map'
pose.pose.position.x = pnt[0]
pose.pose.position.y = pnt[1]
path.poses.append(pose)
#! /usr/bin/env python
import rospy
import sys
#linear and angular velocity
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path
from nav_msgs.msg import Odometry
rospy.init_node('vac_bot', anonymous=False)
true_path = Path()
beli_path = Path()
def big_brain():
rate = rospy.Rate(10) #10Hz
while not rospy.is_shutdown():
move_bot(-0.05, 0.2)
rate.sleep()
def move_bot(lin_vel, ang_vel):
cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=10)
rate = rospy.Rate(10) #10Hz
#Creating Twist message instance
vel = Twist()
vel.linear.x = lin_vel
vel.linear.y = 0
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
flew_in1 = 0
flew_in2 = 0
flew_in3 = 0
cf1_name = 'cf1'
cf2_name = 'cf2'
cf3_name = 'cf3'
human_name = 'palm'
toFly = 0
human_imp = 0
delta_imp = 1
theta_imp = 0
path1 = Path()
path2 = Path()
path3 = Path()
# def tag_game(human, cf1, obstacle1):
# def tag_game(human, cf1, cf2):
#def tag_game(human, cf1, cf2, cf3, obstacle1, obstacle2, obstacle3, obstacle4, obstacle5, obstacle6, obstacle7, obstacle8, obstacle9):
def tag_game(human, cf1, cf2, cf3, obstacle1, obstacle2):
human_pose = swarmlib.get_coord(human)
obstacle1_pose = swarmlib.get_coord(obstacle1)
obstacle2_pose = swarmlib.get_coord(obstacle2)
drone1_pose = swarmlib.get_coord(cf1)
drone2_pose = swarmlib.get_coord(cf2)
drone3_pose = swarmlib.get_coord(cf3)
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
elif type(value) is list:
output[field] = [ros2dict(el) for el in value]
elif type(value) in (np.ndarray, array.array):
output[field] = value.tolist()
else:
output[field] = ros2dict(value)
return output
if __name__ == "__main__":
# Run unit tests
print("str")
print(ros2dict("test"))
print("Path")
from nav_msgs.msg import Path
print(ros2dict(Path()))
print("NavSatFix")
from sensor_msgs.msg import NavSatFix
print(ros2dict(NavSatFix()))
print("Int32MultiArray")
from std_msgs.msg import Int32MultiArray
print(ros2dict(Int32MultiArray()))
print("object (this should not work)")
try:
print(ros2dict(object()))
except ValueError:
print("exception successfully caught")
print("all tests completed successfully")
def allocateNewTasks(data, method='Novel', tras=None):
global robots
global lockd
global humanlock
global currentTimeStump
global startingTimeStump
global costArray
humanlock = True
trajectories = None
tasks = []
for pose in data.poses:
x = pose.position.x
y = pose.position.y
t = [x, y]
tasks.append(t)
rospy.loginfo(tasks)
rospy.loginfo('dragon')
if method == 'Euc':
bids = al.createBids(robots, tasks, method)
startingTimeStump = currentTimeStump
# rospy.loginfo(bids)
allocations = al.auction(bids)
# allocations = [0]
for i, allocation in enumerate(allocations):
if tras is None:
robots[allocation].addTask(tasks[i], small_map, big_map,
rpoints, connections)
else:
robots[allocation].addTask(tasks[i], small_map, big_map,
rpoints, connections,
tras[allocation][i])
p = Path()
p.header.frame_id = "map"
for t in robots[allocation].trajectories[0]:
pose = PoseStamped()
pose.pose.position.x = t[0]
pose.pose.position.y = t[1]
p.poses.append(pose)
pub2.publish(p)
elif method == 'Path':
bids, trajectories = al.createBids(robots,
tasks,
method,
big_map,
small_map,
currentTimeStump,
tras=tras)
startingTimeStump = currentTimeStump
# rospy.loginfo(bids)
allocations = al.auction(bids)
# allocations = [0]
for i, allocation in enumerate(allocations):
robots[allocation].addTask(tasks[i], small_map, big_map, rpoints,
connections,
trajectories[allocation][i])
p = Path()
p.header.frame_id = "map"
for t in robots[allocation].trajectories[0]:
pose = PoseStamped()
pose.pose.position.x = t[0]
pose.pose.position.y = t[1]
p.poses.append(pose)
pub2.publish(p)
else:
bids, trajectories, costArray = al.createBids(robots,
tasks,
method,
big_map,
small_map,
currentTimeStump,
tras=tras)
startingTimeStump = currentTimeStump
# rospy.loginfo(bids)
allocations = al.auction(bids)
# allocations = [0]
for i, allocation in enumerate(allocations):
robots[allocation].addTask(tasks[i], small_map, big_map, rpoints,
connections,
trajectories[allocation][i])
p = Path()
p.header.frame_id = "map"
for t in robots[allocation].trajectories[0]:
pose = PoseStamped()
pose.pose.position.x = t[0]
pose.pose.position.y = t[1]
p.poses.append(pose)
pub2.publish(p)
# input('das')
humanlock = False
# print(humanlock)
lockd = False
return trajectories
# Spin once to update robot state
ee_msg = rospy.wait_for_message(
'/panda_simulator/custom_franka_state_controller/tip_state',
EndPointState)
ee_position = get_endpoint_state(ee_msg)
rospy.loginfo('Doing Forward Integration')
x_traj, x_dot_traj = modulation_svm.forward_integrate_singleGamma_HBS(
ee_position,
DS_attractor[0:3],
learned_gamma,
dt=0.05,
eps=0.03,
max_N=10000)
path_shape = x_traj.shape
rospy.loginfo("Length of plan {}".format(path_shape))
msg = Path()
msg.header.frame_id = "/world"
msg.header.stamp = rospy.Time.now()
rospy.loginfo("Length of plan {}".format(path_shape))
for ii in range(path_shape[0]):
pose = PoseStamped()
pose.pose.position.x = x_traj[ii, 0]
pose.pose.position.y = x_traj[ii, 1]
pose.pose.position.z = x_traj[ii, 2]
msg.poses.append(pose)
rospy.loginfo("Publishing Plan...")
pub_fw_int.publish(msg)
rate.sleep()
##############################################
''' -- DS Parameters -- '''
def publisher():
pub = rospy.Publisher('/path_planning_node/cleaning_plan_nodehandle/cleaning_path', Path, queue_size=1)
rospy.init_node('pose_publisher', anonymous=True)
rate = rospy.Rate(10) # Hz
path = Path()
path.header.frame_id = "map"
path.header.stamp = rospy.Time.now()
path.poses.append(_pose_stamp(0.0, 0.0, 0))
path.poses.append(_pose_stamp(7.5, 0.0, 90))
# path.poses.append(_pose_stamp(7.5, 2.0, -1.5708))
# path.poses.append(_pose_stamp(7.5, 5.0, 90))
# path.poses.append(_pose_stamp(7.5, 9.0, 70))
# path.poses.append(_pose_stamp(8.0, 11.0, 45))
path.poses.append(_pose_stamp(10.0, 12.0, 210))
# path.poses.append(_pose_stamp(10.0, 11.0, 240))
# path.poses.append(_pose_stamp(7.5, 10.0, 270)) # path.poses.append(_pose_stamp(7.5, 10.0, -4.71239))
# path.poses.append(_pose_stamp(7.5, 5.0, 270))
path.poses.append(_pose_stamp(7.5, 0.0, 285))
# path.poses.append(_pose_stamp(8.0, -4.5, 270))
# path.poses.append(_pose_stamp(8.0, -7.0, 270))
path.poses.append(_pose_stamp(8.0, -10.0, 345))
path.poses.append(_pose_stamp(11.5, -11.0, 180))
# path.poses.append(_pose_stamp(11.5, -10.2, 90))
# path.poses.append(_pose_stamp(11.5, -10.2, 90))
###### EV turn ######
# path.poses.append(_pose_stamp(11.5, -10.2, 180))
path.poses.append(_pose_stamp(8.0, -10.0, 90))
# path.poses.append(_pose_stamp(8.0, -7.0, 90))
# path.poses.append(_pose_stamp(8.0, -4.5, 90))
path.poses.append(_pose_stamp(7.5, 0.0, 180))
path.poses.append(_pose_stamp(0.0, 0.0, 0))
##
# # path.poses.append(_pose_stamp(0.0, 0.0, 0))
# path.poses.append(_pose_stamp(7.5, 0.0, 90))
# # path.poses.append(_pose_stamp(7.5, 2.0, -1.5708))
# path.poses.append(_pose_stamp(7.5, 5.0, 90))
# path.poses.append(_pose_stamp(7.5, 9.0, 70))
# path.poses.append(_pose_stamp(8.0, 11.0, 45))
# path.poses.append(_pose_stamp(10.0, 12.0, 210))
# path.poses.append(_pose_stamp(10.0, 11.0, 240))
# path.poses.append(_pose_stamp(7.5, 10.0, 270)) # path.poses.append(_pose_stamp(7.5, 10.0, -4.71239))
# path.poses.append(_pose_stamp(7.5, 5.0, 270))
# path.poses.append(_pose_stamp(7.5, 0.0, 285))
# path.poses.append(_pose_stamp(8.0, -4.5, 270))
# path.poses.append(_pose_stamp(8.0, -7.0, 270))
# path.poses.append(_pose_stamp(8.0, -10.0, 345))
# path.poses.append(_pose_stamp(11.5, -11.0, 90))
# path.poses.append(_pose_stamp(11.5, -10.2, 90))
# ###### EV turn ######
# path.poses.append(_pose_stamp(11.5, -10.2, 180))
# path.poses.append(_pose_stamp(8.0, -10.0, 90))
# path.poses.append(_pose_stamp(8.0, -7.0, 90))
# path.poses.append(_pose_stamp(8.0, -4.5, 90))
# path.poses.append(_pose_stamp(7.5, 0.0, 180))
# path.poses.append(_pose_stamp(0.0, 0.0, 0))
# #
# #
# path.poses.append(_pose_stamp(8.5, 8.5, -0.785398))
# path.poses.append(_pose_stamp(9.5, 9.0, -1.0472))
# path.poses.append(_pose_stamp(10.3, 10.0, -1.0472))
# path.poses.append(_pose_stamp(10.5, 11.0, -1.8326))
# path.poses.append(_pose_stamp(10.0, 12.0, -3.14159))
# path.poses.append(_pose_stamp(9.0, 12.0, -3.66519))
# path.poses.append(_pose_stamp(7.5, 10.0, -4.36332))
# path.poses.append(_pose_stamp(7.5, 10.0, -4.71239))
# path.poses.append(_pose_stamp(7.5, 8.0, -4.71239))
# path.poses.append(_pose_stamp(7.5, 5.0, -4.71239))
# path.poses.append(_pose_stamp(7.5, 2.0, -4.88692))
# path.poses.append(_pose_stamp(8.0, -4.5, -4.71239))
# path.poses.append(_pose_stamp(8.0, -7.0, -4.71239))
# path.poses.append(_pose_stamp(8.0, -10.0, -5.75959))
# path.poses.append(_pose_stamp(11.0, -11.0, -0.785398))
# path.poses.append(_pose_stamp(11.5, -10.0, -1.5708))
# ###### EV turn ######
# path.poses.append(_pose_stamp(11.5, -10.0, -3.1415))
# path.poses.append(_pose_stamp(8.0, -10.0, -0.785398))
# path.poses.append(_pose_stamp(8.0, -10.0, -1.5708))
# path.poses.append(_pose_stamp(8.0, -7.0, -1.5708))
# path.poses.append(_pose_stamp(8.0, -4.5, -1.8326))
# path.poses.append(_pose_stamp(6.0, -0.5, -3.05433))
# path.poses.append(_pose_stamp(0.0, 0.0, 0))
# path.poses.append(_pose_stamp(8.5, 8.5, -0.785398))
# path.poses.append(_pose_stamp(9.5, 9.0, -1.0472))
# path.poses.append(_pose_stamp(10.3, 10.0, -1.0472))
# path.poses.append(_pose_stamp(10.5, 11.0, -1.8326))
# path.poses.append(_pose_stamp(10.0, 12.0, -3.14159))
# path.poses.append(_pose_stamp(9.0, 12.0, -3.66519))
# path.poses.append(_pose_stamp(7.5, 10.0, -4.36332))
# path.poses.append(_pose_stamp(7.5, 10.0, -4.71239))
# path.poses.append(_pose_stamp(7.5, 8.0, -4.71239))
# path.poses.append(_pose_stamp(7.5, 5.0, -4.71239))
# path.poses.append(_pose_stamp(7.5, 2.0, -4.88692))
# path.poses.append(_pose_stamp(8.0, -4.5, -4.71239))
# path.poses.append(_pose_stamp(8.0, -7.0, -4.71239))
# path.poses.append(_pose_stamp(8.0, -10.0, -5.75959))
# path.poses.append(_pose_stamp(11.0, -11.0, -0.785398))
# path.poses.append(_pose_stamp(11.5, -10.0, -1.5708))
# ###### EV turn ######
# path.poses.append(_pose_stamp(11.5, -10.0, -3.1415))
# path.poses.append(_pose_stamp(8.0, -10.0, -0.785398))
# path.poses.append(_pose_stamp(8.0, -10.0, -1.5708))
# path.poses.append(_pose_stamp(8.0, -7.0, -1.5708))
# path.poses.append(_pose_stamp(8.0, -4.5, -1.8326))
# path.poses.append(_pose_stamp(6.0, -0.5, -3.05433))
# path.poses.append(_pose_stamp(0.0, 0.0, 0))
while not rospy.is_shutdown():
connections = pub.get_num_connections()
if connections > 0:
pub.publish(path)
def update_curve(self, data):
# data should contain Path, with multiple, that represent a discretized curve
# we will only use the x,y component for now
points = []
for p in data.poses:
points.append(p.pose.position)
if len(points) < 1:
print('No curve received')
return
line = None
# the segment intersects a circle of radius r if
# the first point is closer than r and the second is further
# we also want the 'last' one that intersects, not the first
# that particular segment is 'forward'.
# p1 inside, p2 outside should not happen for more than 1 point
selfpos = self.pos[:2]
for i in range(len(points) - 1, 0, -1):
p1 = (points[i - 1].x, points[i - 1].y, points[i - 1].z)
p2 = (points[i].x, points[i].y, points[i].z)
p1d = G.euclid_distance(selfpos, p1[:2])
p2d = G.euclid_distance(selfpos, p2[:2])
#print(p1d, p2d)
if p1d > config.LOOK_AHEAD_R:
print('p1d:', p1d, 'p2d:', p2d)
# the first point is inside, check the second one
if p2d < config.LOOK_AHEAD_R:
# we are intersecting!
line = (p1, p2)
if line is None:
print('No segment in range, using first segment')
p1 = (points[0].x, points[0].y, points[0].z)
p2 = (points[1].x, points[1].y, points[1].z)
line = (p1, p2)
p1, p2 = line
p1 = list(p1)
#p1[0] += 3
#p1[1] += 3
line = [p1, p2]
# set these to be used later
self._current_line = line
# self._frame_id = data.header.frame_id
# elongate the line for visualization purposes
x1, y1, z1 = line[0]
x2, y2, z2 = line[1]
slope = (y2 - y1) / (x2 - x1)
d = -5
x2 += d
y2 += d * slope
x1 -= d
y1 -= d * slope
# publish the current line
pose1 = Pose()
pose1.position.x = x1
pose1.position.y = y1
pose1.position.z = z1
stamped1 = PoseStamped()
stamped1.pose = pose1
pose2 = Pose()
pose2.position.x = x2
pose2.position.y = y2
pose2.position.z = z2
stamped2 = PoseStamped()
stamped2.pose = pose2
path = Path()
path.poses = [stamped1, stamped2]
path.header.frame_id = self._frame_id
self.debug_line_pub.publish(path)
def __init__(self):
self.pathPub = rospy.Publisher("path", Path, queue_size=1)
self.path = Path()
def read_waypoints_from_csv(filename):
# Import waypoints.csv into a list (path_points)
poses_waypoints = []
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]
print path_points
for point in path_points:
header = create_header('map')
waypoint = Pose(Point(float(point[0]), float(point[1]), 0),
heading(float(point[2])))
poses_waypoints.append(PoseStamped(header, waypoint))
return poses_waypoints
if __name__ == "__main__":
# Initialize node
rospy.init_node("test_waypoint_publisher")
waypoints_pub = rospy.Publisher('/waypoints_list', Path, queue_size=1)
filename = rospy.get_param('~waypoints_filepath', '')
path = Path()
path.header = create_header('map')
path.poses = read_waypoints_from_csv(filename)
rate = rospy.Rate(0.2)
while not rospy.is_shutdown():
waypoints_pub.publish(path)
rate.sleep()
from gazebo_msgs.msg import ModelState, ModelStates
from nav_msgs.msg import Odometry, Path
from tf import TransformBroadcaster
import rospy
import copy
path_pub = rospy.Publisher('/global_path', Path, latch=True, queue_size=50)
# puntos=[[1,0,90],[2,-3,45],[5.5,4.5,0],[-3,3,0],[-4,-2,0]]
puntos = [[5.8, -4.1, 180], [4, -4.5, 180], [4, -2, 90], [4, -2, 0],
[6, -2, 0], [6, 2, 90], [4.5, 2, 90], [4, 1, 0], [3.5, 1, 0]]
# puntos.reverse()
rospy.init_node('robot_path', anonymous=True)
rate = rospy.Rate(1) # 10hz
h = Header()
path = Path()
pose_t = PoseStamped()
if __name__ == '__main__':
try:
print("dibujando el path en rviz")
h.frame_id = "odom"
path.header = h
pose_t.header = h
while not rospy.is_shutdown():
for i in range(len(puntos)):
pose_t.header.stamp = rospy.Time.now()
pose_t.pose.position.x = puntos[i][0]
pose_t.pose.position.y = puntos[i][1]
(x, y, z, w) = quaternion_about_axis(radians(puntos[i][2]),
(0, 0, 1))
pose_t.pose.orientation.x = x
from std_msgs.msg import String
from std_msgs.msg import Header
from geometry_msgs.msg import TransformStamped
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Pose
from nav_msgs.msg import Path
import rosbag
import csv
bag = rosbag.Bag('sfm_pose.bag', 'w')
stamp_ref = 1413303981.13749303
index_ref = 70
path = Path()
index = 0
try:
with open('localization.csv', 'rb') as csvfile:
csvreader = csv.reader(csvfile, delimiter=',', quotechar='|')
for row in csvreader:
# print row
header = Header()
#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
# Publishes actual path
def actual_path_converter(quadstate):
quat = tf.transformations.quaternion_from_euler(
math.radians(quadstate.roll), math.radians(quadstate.pitch),
math.radians(quadstate.yaw))
pose = Pose(Point(quadstate.x, quadstate.y, quadstate.z),
Quaternion(*quat))
pose_stamped = PoseStamped(Header(0, rospy.Time.now(), "/map"), pose)
actual_path.poses.append(pose_stamped)
pub_actual.publish(actual_path)
pub_actual_marker.publish(pose_stamped)
if __name__ == '__main__':
planned_path = Path()
planned_path.header.frame_id = "/map"
actual_path = Path()
actual_path.header.frame_id = "/map"
pub_planned = rospy.Publisher('visualizer/planned_path',
Path,
queue_size=10)
pub_actual = rospy.Publisher('visualizer/actual_path', Path, queue_size=10)
pub_actual_marker = rospy.Publisher('visualizer/actual_marker',
PoseStamped,
queue_size=10)
pub_planned_marker = rospy.Publisher('visualizer/planned_marker',
PoseStamped,
queue_size=10)
listener()
