GazeSimulation::GazeSimulation(const std::string & name, const ros::NodeHandle & nh) : Gaze(name,nh)
{
private_node_handle.param<std::string>("left_eye_frame", left_eye_frame, "left_eye_frame");
private_node_handle.param<std::string>("right_eye_frame", right_eye_frame, "right_eye_frame");
private_node_handle.param<std::string>("neck_frame", neck_frame, "neck_frame");
private_node_handle.param<std::string>("head_origin_frame", head_origin_frame, "head_origin_frame");
private_node_handle.param<std::string>("eyes_center_frame", eyes_center_frame, "eyes_center_frame");
private_node_handle.param<std::string>("world_frame", world_frame, "world_frame");
// Publishers
neck_pan_pub= nh_.advertise<std_msgs::Float64>("/vizzy/neck_pan_position_controller/command", 1);
neck_tilt_pub=nh_.advertise<std_msgs::Float64>("/vizzy/neck_tilt_position_controller/command", 1);
eyes_tilt_pub=nh_.advertise<std_msgs::Float64>("/vizzy/eyes_tilt_position_controller/command", 1);
l_eye_pub= nh_.advertise<std_msgs::Float64>("/vizzy/l_eye_position_controller/command", 1);
r_eye_pub= nh_.advertise<std_msgs::Float64>("/vizzy/r_eye_position_controller/command", 1);
ROS_INFO("Going to move head and eyes to home position.");
moveHome();
sleep(5.0); // THIS SHOULD CHANGE
ROS_INFO("Done.");
tf::StampedTransform transform;
while(nh_.ok())
{
try
{
tf_listener->waitForTransform(head_origin_frame, neck_frame, ros::Time(0), ros::Duration(10.0) );
tf_listener->lookupTransform(head_origin_frame, neck_frame, ros::Time(0), transform);
tf::Vector3 origin;
origin=transform.getOrigin();
y_offset=origin.getY();
tf_listener->waitForTransform(eyes_center_frame, head_origin_frame, ros::Time(0), ros::Duration(10.0) );
tf_listener->lookupTransform(eyes_center_frame, head_origin_frame, ros::Time(0), transform);
origin=transform.getOrigin();
z_offset=origin.getZ();
}
catch (tf::TransformException &ex)
{
ROS_WARN("%s",ex.what());
continue;
}
break;
}
// Subscribers
neck_pan_sub=boost::shared_ptr<message_filters::Subscriber<control_msgs::JointControllerState> > (new message_filters::Subscriber<control_msgs::JointControllerState>(nh_, "/vizzy/neck_pan_position_controller/state", 10));
neck_tilt_sub=boost::shared_ptr<message_filters::Subscriber<control_msgs::JointControllerState> > (new message_filters::Subscriber<control_msgs::JointControllerState>(nh_, "/vizzy/neck_tilt_position_controller/state", 10));
eyes_tilt_sub=boost::shared_ptr<message_filters::Subscriber<control_msgs::JointControllerState> > (new message_filters::Subscriber<control_msgs::JointControllerState>(nh_, "/vizzy/eyes_tilt_position_controller/state", 10));
fixation_point_sub=boost::shared_ptr<message_filters::Subscriber<geometry_msgs::PointStamped> > (new message_filters::Subscriber<geometry_msgs::PointStamped>(nh_, "fixation_point", 10));
sync=boost::shared_ptr<message_filters::Synchronizer<MySyncPolicy> > (new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(10),
*neck_pan_sub,
*neck_tilt_sub,
*eyes_tilt_sub,
*fixation_point_sub));
sync->registerCallback(boost::bind(&GazeSimulation::analysisCB, this, _1, _2, _3, _4));
as_.registerGoalCallback(boost::bind(&Gaze::goalCB, this));
as_.registerPreemptCallback(boost::bind(&Gaze::preemptCB, this));
as_.start();
}
// Action create
void MapView::createActions()
{
openAction = new QAction(tr("&Open"),this);
openAction->setIcon(QIcon(":/images/open.png"));
openAction->setShortcut(QKeySequence::Open);
openAction->setStatusTip(tr("Open a map file"));
connect(openAction,SIGNAL(triggered()),this,SLOT(open()));
saveAllAction = new QAction(tr("&Save All"),this);
saveAllAction->setIcon(QIcon(":/images/save.png"));
saveAllAction->setShortcut(QKeySequence::Save);
saveAllAction->setStatusTip(tr("Save map and user layer to disk and database"));
connect(saveAllAction,SIGNAL(triggered()),this,SLOT(saveall()));
//file save
saveFileAction = new QAction(tr("Save &File"),this);
saveFileAction->setIcon(QIcon(":/images/save.png"));
saveFileAction->setShortcut(tr("Ctrl+Shift+S"));
saveFileAction->setStatusTip(tr("Save map to disk including user layer"));
connect(saveFileAction,SIGNAL(triggered()),main, SLOT(save()));
//save as
saveAsAction = new QAction(tr("Save &As"),this);
saveAsAction->setIcon(QIcon(":/images/saveas.png"));
saveAsAction->setShortcut(tr("Ctrl+A"));
saveAsAction->setStatusTip(tr("Save map to disk with new file name including user layer"));
connect(saveAsAction,SIGNAL(triggered()),this,SLOT(saveas()));
//load ul from db
GetUserLayerFromDbAction=new QAction(tr("Load U&L from Db"),this);
GetUserLayerFromDbAction->setIcon(QIcon(":/images/fromdb.png"));
GetUserLayerFromDbAction->setStatusTip(tr("Get user layer from database"));
connect(GetUserLayerFromDbAction,SIGNAL(triggered()),main,SLOT(getulfromdb()));
//save ul to db
LoadUserLayerToDbAction =new QAction(tr("Save UL to &Db"),this);
LoadUserLayerToDbAction->setIcon(QIcon(":/images/todb.png"));
LoadUserLayerToDbAction->setStatusTip(tr("Save user layer to database"));
connect(LoadUserLayerToDbAction,SIGNAL(triggered()),main,SLOT(saveultodb()));
makeMapFromDBAction = new QAction(tr("Make map file from Db"), this);
//makeMapFromDBAction->setIcon(QIcon(":/images/todb.png"));
makeMapFromDBAction->setStatusTip(tr("Create Map file from database"));
connect(makeMapFromDBAction,SIGNAL(triggered()),main,SLOT(makeMapFromDb()));
//panel
leftpanelVisibleAction = new QAction(tr("&Layer Panel Hide"),this);
leftpanelVisibleAction->setIcon(QIcon(":/images/layer_blue.png"));
leftpanelVisibleAction->setShortcut(tr("Ctrl+L"));
leftpanelVisibleAction->setStatusTip(tr("hide show layer panel (Ctrl+L)"));
connect(leftpanelVisibleAction,SIGNAL(triggered()),this,SLOT(setLeftPanelVisible()));
objectpanelVisibleAction = new QAction(tr("&Object Panel Hide"),this);
objectpanelVisibleAction->setIcon(QIcon(":/images/panel_blue.png"));
objectpanelVisibleAction->setShortcut(tr("Ctrl+P"));
objectpanelVisibleAction->setStatusTip(tr("hide or show object panel (Ctrl+P)"));
connect(objectpanelVisibleAction,SIGNAL(triggered()),this,SLOT(setObjectPanelVisible()));
//map control
zoomInAction = new QAction(tr("zoom &in"),this);
zoomInAction->setIcon(QIcon(":/images/zoomin.png"));
zoomInAction->setStatusTip(tr("Zoom in map, short key : + "));
connect(zoomInAction,SIGNAL(triggered()),this,SLOT(zoomIn()));
zoomOutAction = new QAction(tr("zoom &out"),this);
zoomOutAction->setIcon(QIcon(":/images/zoomout.png"));
zoomOutAction->setStatusTip(tr("Zoom out map, short key : - "));
connect(zoomOutAction,SIGNAL(triggered()),this,SLOT(zoomOut()));
moveHomeAction = new QAction(tr("se&t Init"),this);
moveHomeAction->setIcon(QIcon(":/images/sethome.png"));
moveHomeAction->setStatusTip(tr("move to initiali zoom and postion"));
connect(moveHomeAction,SIGNAL(triggered()),this,SLOT(moveHome()));
moveLeftAction = new QAction(tr("move &Left"),this);
moveLeftAction->setIcon(QIcon(":/images/arrow_left.png"));
moveLeftAction->setStatusTip(tr("move map to left , short key : arrow left "));
connect(moveLeftAction,SIGNAL(triggered()),this,SLOT(moveLeft()));
moveRightAction = new QAction(tr("move &Right"),this);
moveRightAction->setIcon(QIcon(":/images/arrow_right.png"));
moveRightAction->setStatusTip(tr("move map to right, short key : arrow right"));
connect(moveRightAction,SIGNAL(triggered()),this,SLOT(moveRight()));
moveUpAction = new QAction(tr("move &Up"),this);
moveUpAction->setIcon(QIcon(":/images/arrow_up.png"));
moveUpAction->setStatusTip(tr("move map to upper , short key : arrow up"));
connect(moveUpAction,SIGNAL(triggered()),this,SLOT(moveUp()));
moveDownAction = new QAction(tr("move &Down"),this);
moveDownAction->setIcon(QIcon(":/images/arrow_down.png"));
moveDownAction->setStatusTip(tr("move map to down , short key : arrow down"));
connect(moveDownAction,SIGNAL(triggered()),this,SLOT(moveDown()));
//compass
drawCrossAction = new QAction(tr("show &Compass"),this);
drawCrossAction->setIcon(QIcon(":/images/cross.png"));
drawCrossAction->setStatusTip(tr("show compass "));
drawCrossAction->setCheckable(true);
drawCrossAction->setChecked(false);
connect(drawCrossAction,SIGNAL(triggered(bool)),main,SLOT(showBearingCircle(bool)));
//경위도
drawLatlonAction = new QAction(tr("show &Lat lon"),this);
drawLatlonAction->setIcon(QIcon(":/images/latlon.png"));
drawLatlonAction->setStatusTip(tr("show latitude and longitude"));
drawLatlonAction->setCheckable(true);
drawLatlonAction->setChecked(true);
connect(drawLatlonAction,SIGNAL(triggered(bool)),main,SLOT(showLatLon(bool)));
//id text 표시여부
drawLayerIdAction = new QAction(tr("show Layer &Id"),this);
drawLayerIdAction->setIcon(QIcon(":/images/idvisible.png"));
drawLayerIdAction->setStatusTip(tr("show id text on map"));
drawLayerIdAction->setCheckable(true);
drawLayerIdAction->setChecked(false);
//connect(drawLayerIdAction,SIGNAL(triggered()),this,SLOT(drawLayerId()));
connect(drawLayerIdAction,SIGNAL(triggered(bool)),main,SLOT(onoffLayerIdText(bool)));
savePixmapAction = new QAction(tr("save map &image"),this);
savePixmapAction->setStatusTip(tr("save map to file"));
connect(savePixmapAction,SIGNAL(triggered()),this,SLOT(savePixmap()));
exitAction = new QAction(tr("E&xit"),this);
exitAction->setIcon(QIcon(":/images/quit.png"));
exitAction->setShortcut(tr("Ctrl+Q"));
exitAction->setStatusTip(tr("Exit the application"));
connect(exitAction,SIGNAL(triggered()),this,SLOT(close()));
//거리와 방향을 표시
pointdistanceAction = new QAction(tr("Distance and Direction"),this);
pointdistanceAction->setIcon(QIcon(":/images/pointdistance.png"));
pointdistanceAction->setStatusTip(tr("display distance and direction from a point"));
pointdistanceAction->setCheckable(true);
connect(pointdistanceAction,SIGNAL(triggered(bool)),main,SLOT(onoffDistancBearing(bool)));
// Track Report Menu
trackCreateAction = new QAction(tr("Create"), this);
trackCreateAction->setStatusTip(tr("Track Report Create"));
connect(trackCreateAction, SIGNAL(triggered()), main, SLOT(trackCreate()));
trackSaveAction = new QAction(tr("Save"), this);
trackSaveAction->setStatusTip(tr("Track Report Save"));
connect(trackSaveAction, SIGNAL(triggered()), main, SLOT(trackSave()));
trackOpenAction = new QAction(tr("Open"), this);
trackOpenAction->setStatusTip(tr("Track Report Open"));
connect(trackOpenAction, SIGNAL(triggered()), main, SLOT(trackOpen()));
trackOptionAction = new QAction(tr("Options"), this);
trackOptionAction->setStatusTip(tr("Track Report Options select"));
connect(trackOptionAction, SIGNAL(triggered()), main, SLOT(trackOption()));
trackClTableAction = new QAction(tr("Clear Table"), this);
trackClTableAction->setStatusTip(tr("Track Report Clear Table"));
connect(trackClTableAction, SIGNAL(triggered()), main, SLOT(trackClTable()));
trackClPanelAction = new QAction(tr("Clear Panel"), this);
trackClPanelAction->setStatusTip(tr("Track Report Clear Panel"));
connect(trackClPanelAction, SIGNAL(triggered()), main, SLOT(trackClPanel()));
trackPrintAction = new QAction(tr("Print"), this);
trackPrintAction->setStatusTip(tr("Track Report Print"));
connect(trackPrintAction, SIGNAL(triggered()), main, SLOT(trackPrint()));
}
