int OverlayPickerTool::processMouseEvent(rviz::ViewportMouseEvent& event)
{
if (event.left() && event.leftDown()) {
if (!is_moving_) {
onClicked(event);
}
}
else if (event.left() && is_moving_) {
onMove(event);
}
else if (is_moving_ && !(event.left() && event.leftDown())) {
onRelease(event);
}
return 0;
}
int PolygonTool::processMouseEvent( rviz::ViewportMouseEvent& event )
{
if( !moving_flag_node_ )
{
return Render;
}
Ogre::Vector3 intersection;
Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
if( rviz::getPointOnPlaneFromWindowXY( event.viewport,
ground_plane,
event.x, event.y, intersection ))
{
moving_flag_node_->setVisible( true );
moving_flag_node_->setPosition( intersection );
current_flag_property_->setVector( intersection );
if( event.leftDown() )
{
makeFlag( intersection );
current_flag_property_ = NULL;
return Render | Finished;
}
}
else
{
moving_flag_node_->setVisible( false );
}
return Render;
}
// Handling mouse events
// ^^^^^^^^^^^^^^^^^^^^^
//
// processMouseEvent() is sort of the main function of a Tool, because
// mouse interactions are the point of Tools.
//
// We use the utility function rviz::getPointOnPlaneFromWindowXY() to
// see where on the ground plane the user's mouse is pointing, then
// move the moving flag to that point and update the VectorProperty.
//
// If this mouse event was a left button press, we want to save the
// current flag location. Therefore we make a new flag at the same
// place and drop the pointer to the VectorProperty. Dropping the
// pointer means when the tool is deactivated the VectorProperty won't
// be deleted, which is what we want.
int PottingTool::processMouseEvent( rviz::ViewportMouseEvent& event )
{
if( !moving_flag_node_ )
{
return Render;
}
Ogre::Vector3 intersection;
Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
if( rviz::getPointOnPlaneFromWindowXY( event.viewport,
ground_plane,
event.x, event.y, intersection ))
{
// set the area the obstacle can be settled
if ((abs(intersection.x)<10)&&(abs(intersection.y)<10))
{
moving_flag_node_->setVisible( true );
moving_flag_node_->setPosition( intersection );
current_flag_property_->setVector( intersection );
if( event.leftDown() )
{
makeFlag( intersection );
ofstream myfile("obstacles.txt",ios::app); //example.txt是你要输出的文件的名字
if(!myfile)
{
cout<<"error !";
}
else
{
myfile<<2<<endl<<intersection.x+6.5<<endl<<intersection.y+6.5<<endl;
myfile.close();
}
current_flag_property_ = NULL; // Drop the reference so that deactivate() won't remove it.
return Render | Finished;
}
}
}
else
{
moving_flag_node_->setVisible( false ); // If the mouse is not pointing at the ground plane, don't show the flag.
}
return Render;
}
// Handling mouse events
// ^^^^^^^^^^^^^^^^^^^^^
//
// processMouseEvent() is sort of the main function of a Tool, because
// mouse interactions are the point of Tools.
//
// We use the utility function rviz::getPointOnPlaneFromWindowXY() to
// see where on the ground plane the user's mouse is pointing, then
// move the moving flag to that point and update the VectorProperty.
//
// If this mouse event was a left button press, we want to save the
// current flag location. Therefore we make a new flag at the same
// place and drop the pointer to the VectorProperty. Dropping the
// pointer means when the tool is deactivated the VectorProperty won't
// be deleted, which is what we want.
int DogTool::processMouseEvent( rviz::ViewportMouseEvent& event )
{
if( !moving_flag_node_ )
{
return Render;
}
Ogre::Vector3 intersection;
Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
if( rviz::getPointOnPlaneFromWindowXY( event.viewport,
ground_plane,
event.x, event.y, intersection ))
{
// set the area the obstacle can be settled
if ((abs(intersection.x)<10)&&(abs(intersection.y)<10))
{
moving_flag_node_->setVisible( true );
moving_flag_node_->setPosition( intersection );
current_flag_property_->setVector( intersection );
if( event.leftDown() )
{
makeFlag( intersection );
char *p=getenv("USER");
string username(p);
string filepath="/home/";
filepath+=username;
filepath+="/cafe_robot_single/obstacles.txt";
char* c;
int len = filepath.length();
c =new char[len+1];
strcpy(c,filepath.c_str());
ofstream myfile(c,ios::app);//example.txt是你要输出的文件的名字
filepath="/home/";
filepath+=username;
filepath+="/cafe_robot_single/nav_staff/map/office_map_manual.yaml";
char* d;
len = filepath.length();
d =new char[len+1];
strcpy(d,filepath.c_str());
fstream file;
file.open(d);
string text;
int i=0;
while(i<6)
{
file>>text;
i++;
}
stringstream ss(text);
float f1,f2;
char c1,c2;
ss>>c1>>f1>>c2;
file>>text;
stringstream ss1(text);
ss1>>f2>>c2;
file.close();
if(!myfile)
{
cout<<"error !";
}
else
{
myfile<<4<<endl<<intersection.x-f1<<endl<<intersection.y-f2<<endl;
myfile.close();
}
current_flag_property_ = NULL; // Drop the reference so that deactivate() won't remove it.
return Render | Finished;
}
}
// Handling mouse events
// ^^^^^^^^^^^^^^^^^^^^^
//
// processMouseEvent() is sort of the main function of a Tool, because
// mouse interactions are the point of Tools.
//
// We use the utility function rviz::getPointOnPlaneFromWindowXY() to
// see where on the ground plane the user's mouse is pointing, then
// move the moving flag to that point and update the VectorProperty.
//
// If this mouse event was a left button press, we want to save the
// current flag location. Therefore we make a new flag at the same
// place and drop the pointer to the VectorProperty. Dropping the
// pointer means when the tool is deactivated the VectorProperty won't
// be deleted, which is what we want.
int FlagTool3D::processMouseEvent( rviz::ViewportMouseEvent& event )
{
if( !moving_flag_node_ )
{
return Render;
}
Ogre::Vector3 intersection,intersection1;
Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
if( rviz::getPointOnPlaneFromWindowXY( event.viewport,
ground_plane,
event.x, event.y, intersection ))
{
//moving_flag_node_->setVisible( true );
moving_flag_node_->setPosition( intersection );
current_flag_property_->setVector( intersection );
if( event.leftDown() )
{
std::cout << "Computing projection for " << pcd.size() << " points.\n";
for( size_t i = 0; i < pcd.size(); i++ ) {
Ogre::Vector3 temp3D(pcd.points[i].x,pcd.points[i].y,pcd.points[i].z);
Ogre::Vector2 temp2D = rviz::project3DPointToViewportXY(event.viewport,temp3D);
//std::cout << "\t3D - x: " << temp3D.x << " y: " << temp3D.y << " z: " << temp3D.z << "\t";
//std::cout << "2D - x: " << temp2D.x << " y: " << temp2D.y << "\n";
correspondences.push_back(std::make_pair(temp3D,temp2D));
}
Ogre::Vector2 mouse_pos(event.x,event.y),closest;
std::cout << "Mouse position - x: " << event.x << " y: " << event.y << "\n";
std::cout << "Computing distances for " << correspondences.size() << " points:\n";
double min_dist = 1000000;
for(std::vector<std::pair<Ogre::Vector3,Ogre::Vector2> >::iterator it=correspondences.begin(); it != correspondences.end(); ++it){
std::pair<Ogre::Vector3,Ogre::Vector2> pair = *it;
double dist = FlagTool3D::computeDistance(mouse_pos,pair.second);
//std::cout << "\t2D Point - x: " << pair.second.x << " y: " << pair.second.y << "\tdistance: " << dist << "\n";
if(dist < min_dist) {
min_dist = dist;
intersection1 = pair.first;
closest = pair.second;
}
}
std::cout << "Closest point - x: " << closest.x << " y: " << closest.y << "\n";
std::cout << "Distance: " << min_dist << "\n";
std::cout << "3D point - x: " << intersection1.x << " y: " << intersection1.y << " z: " << intersection1.z << "\n";
moving_flag_node_->setVisible( true );
makeFlag( intersection1 );
//std::cout << "Ogre 3D point - x: " << intersection.x << " y: " << intersection.y << " z: " << intersection.z << "\n";
//std::cout << "--------------------------------------------------------------------------------------------------\n";
//Adding flag pose to the list
geometry_msgs::Pose wp_pose;
wp_pose.position.x = intersection1.x;
wp_pose.position.y = intersection1.y;
wp_pose.position.z = intersection1.z;
wplist.poses.push_back(wp_pose);
current_flag_property_ = NULL; // Drop the reference so that deactivate() won't remove it.
return Render | Finished;
}
if( event.rightDown() ){
//Publishing the flag pose
pub.publish(wplist);
return Render | Finished;
}
}
else
{
moving_flag_node_->setVisible( false ); // If the mouse is not pointing at the ground plane, don't show the flag.
}
correspondences.clear();
return Render;
}
