I'm currently developing an Answer Set Programming problem, consisting in a robot that is needed to cover a room avoiding obstacles and reach a Goal point when all the room is covered.
My idea was to transform the room map into asp predicates,in the form of room/3, being the parameters:
X:x coord
Y:y coord
V:Value of the point in the room, being 0(initial point),1(point to cover),2(Obstacle),3(Goal point)
One of the criteria that the program must meet is to cover every point with a value of 1,which can be achieved with a constraint, but I do not know how to model the robot movement. My idea was to use a predicate of the form move/1,with up,down,left or right.
Can anybody help me guiding me in how to model this problem?
void map_to_asp(std::ofstream& file,std::vector<std::vector<char>>& room)
{
std::cout << room.size() << "," << room[0].size() << std::endl;
for(int i = 0; i < room.size(); i++)
{
for(int j = 0;j < room[0].size(); j++)
{
switch(room[i][j])
{
case '#':
file << "initial(" << i+1 << "," << j+1 << ").\n";
break;
case '.':
file << "toClean(" << i+1 << "," << j+1 << ").\n";
break;
case '#':
file << "obstacle(" << i+1 << "," << j+1 << ").\n";
break;
case 'X':
file << "goal(" << i+1 << "," << j+1 << ").\n";
break;
}
}
}
}
Thank you in advance.
If your goal is to have a model for each possible path, a simple way to go is to make an iterative progression in the graph.
We need first to define all positions we can move in (we are actually building a graph before solving any problem):
position(X,Y,S):- room(X,Y,S) ; not S=2.
Now we decide where we can go from any position (edges of the graph):
edge((X,Y),(I,J)):- position(X,Y,_) ; position(I,J,_) ; |X-I|=0..1 ; |Y-J|=0..1 ; |X-I|+|Y-J|=1..2 .
Note that we consider that the graph is undirected (not necessarily true, if there is a slide in your room for instance).
Let's define some constants:
#const start_pos=(1,1).
#const goal=(5,5).
#const path_maxlen=100.
We obviously start at the starting point:
path(1,start_pos).
And now, we recursively indicate that there is a next way to go, with a limit to avoid too useless solutions.
0{path(N+1,E): path(N,S), edge(S,E), S!=goal}1:- path(N,_) ; N<path_maxlen.
We have to avoid all useless paths.
% a path that do not join the end is illegal.
:- path(N,E) ; not path(N+1,_) ; not E=goal.
% a path must go by all milestone (example of milestone: milestone(2,14)).
:- not path(_,(X,Y)): milestone(X,Y).
We want the shortest path:
last_step(N):- path(N,_) ; not path(N+1,_).
#minimize{N: last_step(N)}.
The full code is available here.
As a side-notes:
since we don't use them, you could (should) take rid of all room/3 that describe an obstacle.
you could also make you goal point artificial (out of the room, but the real goad is linked to it) in order to allow your path to pass by the real goal, without stopping. Using that, you can achieve support for multiple goal.
Related
Hi i'm new in pointcloud library. I'm trying to show clustering result point on rviz or pcl viewer, and then show nothing. And i realize that my data show nothing too when i subcsribe and cout that. Hopefully can help my problem, thanks
This is my code for clustering and send node
void cloudReceive(const sensor_msgs::PointCloud2ConstPtr& inputMsg){
mutex_lock.lock();
pcl::fromROSMsg(*inputMsg, *inputCloud);
cout<<inputCloud<<endl;
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud(inputCloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance(0.03);//2cm
ec.setMinClusterSize(200);//min points
ec.setMaxClusterSize(1000);//max points
ec.setSearchMethod(tree);
ec.setInputCloud(inputCloud);
ec.extract(cluster_indices);
if(cluster_indices.size() > 0){
std::vector<pcl::PointIndices>::const_iterator it;
int i = 0;
for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it){
if(i >= 10)
break;
cloud_cluster[i]->points.clear();
std::vector<int>::const_iterator idx_it;
for (idx_it = it->indices.begin(); idx_it != it->indices.end(); idx_it++)
cloud_cluster[i]->points.push_back(inputCloud->points[*idx_it]);
cloud_cluster[i]->width = cloud_cluster[i]->points.size();
// cloud_cluster[i]->height = 1;
// cloud_cluster[i]->is_dense = true;
cout<<"PointCloud representing the Cluster: " << cloud_cluster[i]->points.size() << " data points"<<endl;
std::stringstream ss;
ss<<"cobaa_pipecom2_cluster_"<< i << ".pcd";
writer.write<pcl::PointXYZRGB> (ss.str(), *cloud_cluster[i], false);
pcl::toROSMsg(*cloud_cluster[i], outputMsg);
// cout<<"data = "<< outputMsg <<endl;
cloud_cluster[i]->header.frame_id = FRAME_ID;
pclpub[i++].publish(outputMsg);
// i++;
}
}
else
ROS_INFO_STREAM("0 clusters extracted\n");
}
And this one is the main
int main(int argc, char** argv){
for (int z = 0; z < 10; z++) {
// std::cout << " - clustering/" << z << std::endl;
cloud_cluster[z] = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
cloud_cluster[z]->height = 1;
cloud_cluster[z]->is_dense = true;
// cloud_cluster[z]->header.frame_id = FRAME_ID;
}
ros::init(argc,argv,"clustering");
ros::NodeHandlePtr nh(new ros::NodeHandle());
pclsub = nh->subscribe("/pclsegmen",1,cloudReceive);
std::string pub_str("clustering/0");
for (int z = 0; z < 10; z++) {
pub_str[11] = z + 48;//48=0(ASCII)
// z++;
pclpub[z] = nh->advertise <sensor_msgs::PointCloud2> (pub_str, 1);
}
// pclpub = nh->advertise<sensor_msgs::PointCloud2>("/pclcluster",1);
ros::spin();
}
This isn't an exact answer, but I think it addresses your issue & may ease your debugging.
RViz can directly subscribe to a published point cloud, the one I'm assuming you're trying to see in the cloud_receive callback. If you set the Frame to whichever frame it's being published at, and add it from the available topics, you should see the points. (Easier than trying to rebroadcast it as different topics).
Also, I recommend looking at the rostopic command line tool. You can do rostopic list to check if it's being published, rostopic bw to see if it's really publishing the expected volume of data (ex bytes vs kilobytes vs megabytes), rostopic hz to see how frequently (if ever) it's publishing, and (briefly) rostopic echo to look at the data itself. (This is me assuming from your question it's more an issue with the data coming into your node).
If you're having trouble, not with data coming into the node, nor with the visualization of pointcloud data in general, but with the transformed data that's supposed to come out of the node, I would check that the clustering worked, & reduce your code moreso to just having 1 publisher publish something. You may be doing something weird. Like messing up your pointers. You could also turn on stronger compilation warnings for your node with -Wall -Wextra -Werror or step through the execution of it via gdb (launch-prefix="xterm -e gdb --args").
The solution is, i change the ASCII number into lexical_cast. Thanks for your response, i hope this can help other
for (int z = 0; z < CLOUD_QTD; z++) {
// pub_str[11] = z + 48;
std::string topicName = "/pclcluster/" + boost::lexical_cast<std::string>(z);
global::pub[z] = n.advertise <sensor_msgs::PointCloud2> (topicName, 1);
}
I'm using Z3 C++ API (Version 4.3.1) and I want to extract variables of a formula (An object of type expr). I've found a similar question but it is in Z3py. I am wonder if there is a method in Z3 C/C++ API to extract variables from expr object. Thanks!
For example (some details omitted):
expr fs = implies(x + y == 0, z * x < 15);
std::vector<expr> varlist = get_vars(fs);
Then varlist should contain x,y,z.
The C++ example in the distribution (examples/c++/example.cpp) shows a sample visitor pattern.
It is very simplistic, but will give the idea.
I repeat it here below:
void visit(expr const & e) {
if (e.is_app()) {
unsigned num = e.num_args();
for (unsigned i = 0; i < num; i++) {
visit(e.arg(i));
}
// do something
// Example: print the visited expression
func_decl f = e.decl();
std::cout << "application of " << f.name() << ": " << e << "\n";
}
else if (e.is_quantifier()) {
visit(e.body());
// do something
}
else {
assert(e.is_var());
// do something
}
}
The visitor function can be improved by using
a cache of previously visited expressions because in general Z3 uses shared sub-expressions.
This is similar to the Python example.
Hope this helps
I've a vector of points from a grey section of an image and written like this:
std::vector<Point> vectorg;
for(i = 0; i <= hei - 1; i++) {
for(j = 0; j <= wid - 1; j++) {
if(mask(i,j) == 128) {
vectorg.push_back(Point(j,i));
}
}
}
Knowing what coordinates stored in certain cell is possible by:
cout << vectorg[0].x;
cout << vectorg[0].y;
The question is now the other way around, is it possible to know which cell holds certain coordinates?
Thanks a lot, I'm new here also with opencv programming, I'll be in your care.
Just do the following:
#include <algorithm>
// ...
Point p(searchedX, searchedY);
std::vector<Point>::iterator element = std::find(vectorg.begin(), vectorg.end(), p);
if (element != vectorg.end()) {
cout << (*element).x << endl;
cout << (*element).y << endl;
} else {
cout << "The point is not in the vector" << endl;
}
It may be overkill, but a way to do it (without doing a greedy exhaustive search) would be to build a FLANN index that will store the position of your points.
The feature matrix is made of the coordinates of your points.
Since OpenCV knows how to convert a vector to a matrix, you should be able to use your current vector as is.
Then, if you want only one point, just ask for the 1 nearest neighbour in the query (k parameter).
The bonus is, if you decide later that you need to have also the closest points in the neighborhood, just raise the value of k.
Sorry for the late response, and thanks for the answers, they inspired me indirectly.
I found an easy way to do it. This is by making another Mat which holds the numbers where the coordinates were saved.
std::vector<Point> vectorg;
cv::Mat_<int> Index =Mat_<int>::zeros(hei,wid);
for(i = 0; i <= hei - 1; i++) {
for(j = 0; j <= wid - 1; j++) {
if(mask(i,j) == 128) {
vectorg.push_back(Point(j,i));
Index(vector[count])=count;
count++;
}
}
}
This way I can know which cell holds certain coordinates by simply:
cout<<Index(36,362); //as example
Thanks a lot, I'll be in your care next time.
I have a fairly basic program that is intended to sort a list of numbers via a Linked List.
Where I am getting hung up is when the element needs to be inserted at the beginning of the list. Here is the chunk of code in question
Assume that root->x = 15 and assume that the user inputs 12 when prompted:
void addNode(node *root)
{
int check = 0; //To break the loop
node *current = root; //Starts at the head of the linked list
node *temp = new node;
cout << "Enter a value for x" << endl;
cin >> temp->x;
cin.ignore(100,'\n');
if(temp->x < root->x)
{
cout << "first" << endl;
temp->next=root;
root=temp;
cout << root->x << " " << root->next->x; //Displays 12 15, the correct response
}
But if, after running this function, I try
cout << root->x;
Back in main(), it displays 15 again. So the code
root=temp;
is being lost once I leave the function. Now other changes to *root, such as adding another element to the LL and pointing root->next to it, are being carried over.
Suggestions?
This because you are setting the local node *root variable, you are not modifying the original root but just the parameter passed on stack.
To fix it you need to use a reference to pointer, eg:
void addNode(node*& root)
or a pointer to pointer:
void addNode(node **root)
How would I go about displaying detailed distance between words.
For example, the output of the program could be:
Words are "car" and "cure":
Replace "a" with "u".
Add "e".
The Levenshtein distance does not fulfill my needs (I think).
Try the following. The algorithm is roughly following Wikipedia (Levenshtein distance). The language used below is ruby
Use as an example, the case of changing s into t as follows:
s = 'Sunday'
t = 'Saturday'
First, s and t are turned into arrays, and an empty string is inserted at the beginning. m will eventually be the matrix used in the argorithm.
s = ['', *s.split('')]
t = ['', *t.split('')]
m = Array.new(s.length){[]}
m here, however, is different from the matrix given if the algorithm in wikipedia for the fact that each cell includes not only the Levenshtein distance, but also the (non-)operation (starting, doing nothing, deletion, insertion, or substitution) that was used to get to that cell from an adjacent (left, up, or upper-left) cell. It may also include a string describing the parameters of the operation. That is, the format of each cell is:
[Levenshtein distance, operation(, string)]
Here is the main routine. It fills in the cells of m following the algorithm:
s.each_with_index{|a, i| t.each_with_index{|b, j|
m[i][j] =
if i.zero?
[j, "started"]
elsif j.zero?
[i, "started"]
elsif a == b
[m[i-1][j-1][0], "did nothing"]
else
del, ins, subs = m[i-1][j][0], m[i][j-1][0], m[i-1][j-1][0]
case [del, ins, subs].min
when del
[del+1, "deleted", "'#{a}' at position #{i-1}"]
when ins
[ins+1, "inserted", "'#{b}' at position #{j-1}"]
when subs
[subs+1, "substituted", "'#{a}' at position #{i-1} with '#{b}'"]
end
end
}}
Now, we set i, j to the bottom-right corner of m and follow the steps backwards as we unshift the contents of the cell into an array called steps, until we reach the start.
i, j = s.length-1, t.length-1
steps = []
loop do
case m[i][j][1]
when "started"
break
when "did nothing", "substituted"
steps.unshift(m[i-=1][j-=1])
when "deleted"
steps.unshift(m[i-=1][j])
when "inserted"
steps.unshift(m[i][j-=1])
end
end
Then we print the operation and the string of each step unless that is a non-operation.
steps.each do |d, op, str=''|
puts "#{op} #{str}" unless op == "did nothing" or op == "started"
end
With this particular example, it will output:
inserted 'a' at position 1
inserted 't' at position 2
substituted 'n' at position 2 with 'r'
class Solution:
def solve(self, text, word0, word1):
word_list = text.split()
ans = len(word_list)
L = None
for R in range(len(word_list)):
if word_list[R] == word0 or word_list[R] == word1:
if L is not None and word_list[R] != word_list[L]:
ans = min(ans, R - L - 1)
L = R
return -1 if ans == len(word_list) else ans
ob = Solution()
text = "cat dog abcd dog cat cat abcd dog wxyz"
word0 = "abcd"
word1 = "wxyz"
print(ob.solve(text, word0, word1))