Button::Button(Point position, int width, int high) : Element(position, width, high)
{
unHighlight();
}
CameraView::CameraView( QWidget* parent, Base3DView* copy_from )
: Base3DView( copy_from, "/world", "CameraView", parent )
, camera_frame_topic_("")
, feed_rate_(0)
, feed_resolution_(4)
, area_rate_(0)
, area_resolution_(0)
, setting_pose_(false)
, selection_made_(false)
, initialized_(false)
, selection_tool_enabled_( true )
, current_pitch_(0)
{
this->setMouseTracking(true);
// Load camera topics from parameter server
loadCameraTopics("/flor/ocs/camera/atlas");
loadCameraTopics("/flor/ocs/camera/left_hand");
loadCameraTopics("/flor/ocs/camera/right_hand");
// Make sure we have camera topics
if(!camera_.size())
{
ROS_ERROR("Need to load camera topics in the parameter server. Try using:\n\troslaunch vigir_ocs vigir_ocs.launch\t\t- for the OCS UI\n\troslaunch vigir_ocs vigir_ocs_camera.launch\t- for the camera widgets standalone");
exit(1);
}
// Create a camera/image display.
camera_viewer_ = manager_->createDisplay( "rviz/CameraDisplayCustom", "Camera image", true ); // this would use the plugin instead of manually adding the display object to the manager
ROS_ASSERT( camera_viewer_ != NULL );
((rviz::CameraDisplayCustom*)camera_viewer_)->setRenderPanel( render_panel_ );
((rviz::CameraDisplayCustom*)camera_viewer_)->setup();
((rviz::CameraDisplayCustom*)camera_viewer_)->setViewID(view_id_);
// Add support for selection
selection_tool_ = manager_->getToolManager()->addTool( "rviz/ImageSelectionToolCustom" );
std::bitset<32> y(camera_viewer_->getVisibilityBits());
std::cout << "camera vis bit: " << y << std::endl;
uint32_t vis_bit = 0xFFFFFFFF;
for(int i = 0; i < 4; i++)
{
std::bitset<32> a((uint32_t)pow(2,i));
std::cout << "\tsetting: " << a << std::endl;
if(view_id_ == i)
vis_bit |= (uint32_t)pow(2,i); // send the viewport visilibity bit to the display and display will know if it should render there or not.
else
vis_bit &= ~(uint32_t)pow(2,i);
}
std::bitset<32> x(vis_bit);
std::cout << "tool vis bit: " << x << std::endl;
((rviz::ImageSelectionToolCustom*)selection_tool_)->setVisibilityBits(vis_bit);
((rviz::CameraDisplayCustom*)camera_viewer_)->setVisibilityBits(vis_bit);
// connect the selection tool select signal to this
QObject::connect(selection_tool_, SIGNAL(select(int,int,int,int)), this, SLOT(select(int,int,int,int)));
manager_->getToolManager()->setCurrentTool( selection_tool_ );
if(!copy_from)
{
robot_model_->setEnabled(false);
ghost_robot_model_->setEnabled(false);
}
QObject::connect(this, SIGNAL(setFullImageResolution(int)), camera_viewer_, SLOT(changeFullImageResolution(int)));
QObject::connect(this, SIGNAL(setCropImageResolution(int)), camera_viewer_, SLOT(changeCropImageResolution(int)));
QObject::connect(this, SIGNAL(setCameraSpeed(int)), camera_viewer_, SLOT(changeCameraSpeed(int)));
QObject::connect(this, SIGNAL(setCropCameraSpeed(int)), camera_viewer_, SLOT(changeCropCameraSpeed(int)));
QObject::connect(this, SIGNAL(publishCropImageRequest()), camera_viewer_, SLOT(publishCropImageRequest()));
QObject::connect(this, SIGNAL(publishFullImageRequest()), camera_viewer_, SLOT(publishFullImageRequest()));
QObject::connect(this, SIGNAL(unHighlight()), selection_tool_, SLOT(unHighlight()));
QObject::connect(camera_viewer_, SIGNAL(updateFrameID(std::string)), this, SLOT(updateImageFrame(std::string)));
Q_EMIT unHighlight();
if(!nh_.getParam("pitch_joint", joint_name_))
{
joint_name_ = "neck_ry";
}
// and advertise the head pitch update function
//head_pitch_update_pub_ = nh_.advertise<std_msgs::Float64>( "/atlas/pos_cmd/neck_ry", 1, false );
head_pitch_update_traj_pub_ = nh_.advertise<trajectory_msgs::JointTrajectory > ("/trajectory_controllers/neck_traj_controller/command",1,false);
rviz::EmptyViewController* camera_controller = new rviz::EmptyViewController();
camera_controller->initialize( render_panel_->getManager() );
render_panel_->setViewController( camera_controller );
close_area_button_ = new QPushButton("X",this);
QObject::connect(close_area_button_, SIGNAL(clicked()), this, SLOT(closeSelectedArea()));
close_area_button_->hide();
close_area_button_->setStyleSheet(QString("QPushButton { ") +
" background-color: qlineargradient(spread:pad, x1:0, y1:0, x2:0, y2:1, stop:0 rgba(240, 240, 240, 255), stop:1 rgba(222, 222, 222, 255));" +
" border-style: solid;" +
" border-width: 1px;" +
" border-radius: 1px;" +
" border-color: gray;" +
" padding: 0px;" +
" font: \"Ubuntu\";"
" font-size: 9px;"
"}" +
"QPushButton:pressed {" +
" padding-top:1px; padding-left:1px;" +
" background-color: rgb(180,180,180);" +
" border-style: inset;" +
"}");
QObject::connect((selection_tool_), SIGNAL(mouseHasMoved(int,int)), this, SLOT(mouseMoved(int,int)));
// set scale of the selection marker
selection_3d_display_->subProp("Scale")->setValue(0.001f);
selection_3d_display_->subProp("Constant Visual Size")->setValue(false);
// advertise pointcloud request
pointcloud_request_frame_pub_ = nh_.advertise<geometry_msgs::PointStamped>( "/flor/worldmodel/ocs/dist_query_pointcloud_request_frame", 1, false );
//reset_view_button_->setParent(0);
//delete reset_view_button_;
//reset_view_button_ = NULL;
reset_view_button_->hide();
// make sure we're still able to cancel set goal pose
QObject::connect(render_panel_, SIGNAL(signalKeyPressEvent(QKeyEvent*)), this, SLOT(keyPressEvent(QKeyEvent*)));
QObject::connect(render_panel_, SIGNAL(signalMouseEnterEvent(QEvent*)), this, SLOT(mouseEnterEvent(QEvent*)));
QObject::connect(render_panel_, SIGNAL(signalMouseMoveEvent(QMouseEvent*)), this, SLOT(mouseMoveEvent(QMouseEvent*)));
Q_FOREACH( QWidget* sp, findChildren<QWidget*>() ) {
sp->installEventFilter( this );
sp->setMouseTracking( true );
}
void MSPrimitive::focusOut(void)
{ unHighlight(); }
void MSComposite::focusOut(void)
{
unHighlight();
}
