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;
}
Ogre::Vector3 PoseTool::getPositionFromMouseXY( Ogre::Viewport* viewport, int mouse_x, int mouse_y )
{
int width = viewport->getActualWidth();
int height = viewport->getActualHeight();
Ogre::Ray mouse_ray = viewport->getCamera()->getCameraToViewportRay( (float)mouse_x / (float)width, (float)mouse_y / (float)height );
Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects( ground_plane );
if ( !intersection.first )
{
return Ogre::Vector3::ZERO;
}
return mouse_ray.getPoint( intersection.second );
}
bool XYOrbitViewController::intersectGroundPlane( Ogre::Ray mouse_ray, Ogre::Vector3 &intersection_3d )
{
//convert rays into reference frame
mouse_ray.setOrigin( target_scene_node_->convertWorldToLocalPosition( mouse_ray.getOrigin() ) );
mouse_ray.setDirection( target_scene_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * mouse_ray.getDirection() );
Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0 );
std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects(ground_plane);
if (!intersection.first)
{
return false;
}
intersection_3d = mouse_ray.getPoint(intersection.second);
return true;
}
void PovExp::convert(std::string filename ){
FILE * out;
std::string file = filename + extension;
out = fopen( file.c_str(),"w" );
header(out);
camera(out,0,3,2,15,0,0,1,1,1);
parallel_light(out,0,5,5,15,15,15,1,1,1);
ground_plane(out,0,0.5,0.5,0.5);
lego(out, "Lego_2x2",0,0,0,0,1,0,0);
lego(out, "Lego_1x3",3,0,3,90,1,1,0);
fclose(out);
//camera(out, TX, TY, TZ, RX, RY, RZ, SX, SY, SZ);
}
// 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;
}
void RenderField(Field *field) {
Lightcycle *players = field->player;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
field_w = field->width;
field_h = field->height;
place_camera(field);
ground_plane(field_w, field_h);
render_axes();
while (players != 0) {
render_player(players);
players = players->next;
}
glFlush();
}
int PoseTool::processMouseEvent( ViewportMouseEvent& event )
{
int flags = 0;
if( event.leftDown() )
{
ROS_ASSERT( state_ == Position );
Ogre::Vector3 intersection;
Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
if( getPointOnPlaneFromWindowXY( event.viewport,
ground_plane,
event.x, event.y, intersection ))
{
pos_ = intersection;
arrow_->setPosition( pos_ );
state_ = Orientation;
flags |= Render;
}
}
else if( event.type == QEvent::MouseMove && event.left() )
{
if( state_ == Orientation )
{
//compute angle in x-y plane
Ogre::Vector3 cur_pos;
Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
if( getPointOnPlaneFromWindowXY( event.viewport,
ground_plane,
event.x, event.y, cur_pos ))
{
double angle = atan2( cur_pos.y - pos_.y, cur_pos.x - pos_.x );
arrow_->getSceneNode()->setVisible( true );
//we need base_orient, since the arrow goes along the -z axis by default (for historical reasons)
Ogre::Quaternion orient_x = Ogre::Quaternion( Ogre::Radian(-Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_Y );
arrow_->setOrientation( Ogre::Quaternion( Ogre::Radian(angle), Ogre::Vector3::UNIT_Z ) * orient_x );
flags |= Render;
}
}
}
else if( event.leftUp() )
{
if( state_ == Orientation )
{
//compute angle in x-y plane
Ogre::Vector3 cur_pos;
Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
if( getPointOnPlaneFromWindowXY( event.viewport,
ground_plane,
event.x, event.y, cur_pos ))
{
double angle = atan2( cur_pos.y - pos_.y, cur_pos.x - pos_.x );
onPoseSet(pos_.x, pos_.y, angle);
flags |= (Finished|Render);
}
}
}
return flags;
}
// 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;
}
