// In a tool's load() function, we don't need to read its class
// because that has already been read and used to instantiate the
// object before this can have been called.
void FlagTool3D::load( const rviz::Config& config )
{
// Here we get the "Flags" sub-config from the tool config and loop over its entries:
rviz::Config flags_config = config.mapGetChild( "Flags" );
int num_flags = flags_config.listLength();
for( int i = 0; i < num_flags; i++ )
{
rviz::Config flag_config = flags_config.listChildAt( i );
// At this point each ``flag_config`` represents a single flag.
//
// Here we provide a default name in case the name is not in the config file for some reason:
QString name = "Flag " + QString::number( i + 1 );
// Then we use the convenience function mapGetString() to read the
// name from ``flag_config`` if it is there. (If no "Name" entry
// were present it would return false, but we don't care about
// that because we have already set a default.)
flag_config.mapGetString( "Name", &name );
// Given the name we can create an rviz::VectorProperty to display the position:
rviz::VectorProperty* prop = new rviz::VectorProperty( name );
// Then we just tell the property to read its contents from the config, and we've read all the data.
prop->load( flag_config );
// We finish each flag by marking it read-only (as discussed
// above), adding it to the property container, and finally making
// an actual visible flag object in the 3D scene at the correct
// position.
prop->setReadOnly( true );
getPropertyContainer()->addChild( prop );
makeFlag( prop->getVector() );
}
}
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;
}
void TclClass::sendColor(byte red, byte green, byte blue) {
byte flag;
flag = makeFlag(red,green,blue);
sendFrame(flag,red,green,blue);
}
// 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 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;
}
