void reportStatus() {
char json[JSON_MAX_SIZE];
if (nextEntryReport == 0) {
// start reporting
nextEntryReport = SENSOR_TEMP;
}
switch (nextEntryReport) {
case SENSOR_TEMP:
jsonifySensorValue(SENSOR_TEMP, tempRoom, json, JSON_MAX_SIZE);
broadcastMsg(json);
nextEntryReport = SENSOR_HUM;
break;
case SENSOR_HUM:
jsonifySensorValue(SENSOR_HUM, humRoom, json, JSON_MAX_SIZE);
broadcastMsg(json);
nextEntryReport = 0;
break;
default:
break;
}
}
void CameraController::onRecvMsg(RecvMsgEvent &evt) {
std::string msg = evt.getMsg();
std::vector<MoveData> moves = buildMoveData(msg);
if (!moves.empty()) { // seg fault if task is not started
MoveData move = moves[0]; // only one move needed for this operation
if (move.releasedButtons & Btn_MOVE) {
if (! configuringPath) {
rotateTowardGround();
std::cout << "starting path definition" << std::endl;
configuringPath = true;
}
else {
// save path --> write path to file
if (!path.empty()) {
std::ofstream output;
std::stringstream filename;
time_t now = time(0);
struct tm tstruct;
char buf[80];
tstruct = *localtime(&now);
strftime(buf, sizeof(buf), "%Y-%m-%d.%X", &tstruct);
filename << "nodes/"
<< buf
<< ".txt";
output.open(filename.str().c_str());
std::vector<Vector3d>::iterator node;
for (node = path.begin(); node != path.end(); ++node) {
std::cout << "Node : " << node->x() << ", " << node->y() << ", " << node->z() << std::endl;
output << node->x() << "," << node->y() << "," << node->z() << "," << "0" << std::endl;
}
output.close();
std::stringstream msg;
msg << "new path wrote to " << filename.str();
std::cout << msg << std::endl;
broadcastMsg(msg.str());
}
path.clear();
configuringPath = false;
}
}
else if (configuringPath) {
// update the position of the cursor.
SimObj* cursor = getObj("pointer1");
double xPosition = -300*move.tracker.y;
double yPosition = 100;
double zPosition = -300*move.tracker.x;
cursor->setPosition(xPosition, yPosition, zPosition);
if (move.releasedButtons & Btn_CROSS) {
// add a step to the current path
path.push_back(Vector3d(xPosition, yPosition, zPosition));
}
else if (move.releasedButtons & Btn_CIRCLE) {
// remove the last step of the path
if (! path.empty()) {
path.pop_back();
}
}
}
}
}
void MyController::onCollision(CollisionEvent &evt)
{
std::string msg = "Collision";
broadcastMsg(msg);
}
double MyController::onAction(ActionEvent &evt)
{
if(first == false){
std::string msg = "start";
broadcastMsg(msg);
first = true;
}
int count=0;
double r=0.0; //2点間の直線距離
double angle;
if(end==false){
if(start==true){
Vector3d pos;
Vector3d npos;
if(first==false){
FILE* fp;
x=0;
y=0;
z=0;
w=0; //チェックポイント
dx=0;
dy=0;
dz=0;
if((fp = fopen("node.txt", "r")) == NULL) {
printf("File do not exist.\n");
exit(0);
}
while(fscanf(fp, "%lf,%lf,%lf,%lf", &x, &y, &z,&w) != EOF) {
temp.x[i]=x;
temp.y[i]=y;
temp.z[i]=z;
temp.w[i]=w;
i++;
}
fclose(fp);
first = true;
i=0;
}
if(stop==false){
my->getPosition(pos);
npos.x(temp.x[i]);
npos.y(temp.y[i]);
npos.z(temp.z[i]);
angle = rotateTowardObj(npos);
if(angle < 0.0){
angle = -1.0 * angle;
}
/*ここに相当する部分を書く*/
//my->setAxisAndAngle(0, 1.0, 0, -angle);
// 回転すべき円周距離
double distance = m_distance*PI*fabs(targetAngle)/(2*PI);
if(targetAngle > 0.0){
m_my->setWheelVelocity(velocity, -velocity);
}
else{
m_my->setWheelVelocity(-velocity, velocity);
}
// 車輪の半径から移動速度を得る
double vel = m_radius*velocity;
// 回転時間(u秒)
double time = (distance / vel) + evt.time();
if(evt.time>=time){
m_my->setWheelVelocity(0, 0);
}
dx=(temp.x[i]-pos.x());
dy=(temp.y[i]-pos.y());
dz=(temp.z[i]-pos.z());
r=sqrt(pow(dx,2)+pow(dz,2));
//ここまでが現在地から次の座標までの距離と角度の計算
double vel = m_radius*velocity;
// 移動開始
m_my->setWheelVelocity(velocity, velocity);
// 到着時間取得
double time2 = r / vel;
count = 0;
count2= 0;
i++;
}
if(temp.w[i-1] == 1.0){
std::string msg = "point1";
//"robot_000"にメッセージを送信します
sendMsg("man_001", msg);
}else if(temp.w[i-1] == 2.0){
stop=true;
broadcastMsgToSrv("elevator");
if(elevator==true){
stop=false;
}
}
}
}
/***************************************************************************/
/*
if(strstr(myname() , "robot_004")!= NULL){
m_my->setWheelVelocity(m_vel*2, m_vel*2);
}
*/
return 0.1;
}
void SendBroadcastForm::onSendBtn(){
emit broadcastMsg(ui->textEdit->toPlainText());
ui->textEdit->clear();
}
void QRoseComponent::broadcastCache(int groupId) {
if (m_cache.empty()) return;
SgNodesMsg msg(&m_cache);
broadcastMsg(&msg, groupId);
}
void QRoseComponent::broadcastNode(SgNode *node, int groupId) {
SgNodeMsg msg(node);
broadcastMsg(&msg, groupId);
}
void OnMessage(const khaki::TcpClientPtr& con) {
khaki::Buffer tmp(con->getBuf());
broadcastMsg(tmp);
}
double RobotController::onAction(ActionEvent &evt)
{
//std::cout << "m_state " << m_state << std::endl;
//std::cout << "the size of Vector " << Record_Postures.size() << std::endl;
switch(m_state)
{
case 10:
{
Robot_speed = Change_Robot_speed;
choose_task_arm(5, LEFT_ARM);
choose_task_arm(5, RIGHT_ARM);
// printf("got it in case!1 flag1 \n");
if (goTo(m_relayPoint1, 0) == true && moveArm(LEFT_ARM) == true && moveArm(RIGHT_ARM) == true)
{
m_state = 20;
// printf("got it in case!1 \n");
}
break;
}
case 20:
{
Robot_speed = Change_Robot_speed;
if (goTo(m_relayPoint2, 0) == true) m_state = 30;
break;
}
case 30:
{
Robot_speed = Change_Robot_speed;
if (goTo(m_relayFrontTable, 0) == true) m_state = 40;
break;
}
case 40: // Test if the cycle is finished or not
{
if (cycle > 0)
{
m_state = 41;
broadcastMsg("Show_me");
get_Kinect_Data();
m_time = evt.time() + 5;
break;
}
else
{
m_state = 49;
break;
}
}
case 41:
{
if(evt.time() > m_time) m_state = 42;
break;
}
case 42:
{
Kinect_Data_Off(); // finished analysing data
m_pointedObject = "petbottle";
m_pointedtrash = "recycle";
std::cout << "Task started Robot ........ " << std::endl;
//PrintPosture();
m_state = 50;
}
case 49:
{
break;
}
case 50: //Optional case !!!
{
Robot_speed = Change_Robot_speed;
if (m_pointedObject=="petbottle")
{
if (goTo(m_BottleFront, 0) == true) m_state = 60;
}
else if (m_pointedObject=="mugcup")
{
if (goTo(m_MuccupFront, 0) == true) m_state = 60;
}
else if (m_pointedObject=="can")
{
if (goTo(m_CanFront, 0) == true) m_state = 60;
}
break;
}
case 60: //preparation of the arm for grasp
{
Robot_speed = Change_Robot_speed;
recognizeObjectPosition(m_Object_togo, m_pointedObject);
if (goTo(m_Object_togo, 70) == true)
{
m_state = 70;
}
break;
}
case 70: //preparation of the arm for grasp
{
choose_task_arm(1, LEFT_ARM);
if (moveArm(LEFT_ARM) == true) m_state = 80;
break;
}
case 80: //move to the object
{
Robot_speed = 1;
if (goTo(m_Object_togo, 38) == true) m_state = 90;
break;
}
case 90: //move arm to grasp the object
{
choose_task_arm(2, LEFT_ARM);
grasp_left_hand();
if (moveArm(LEFT_ARM) == true) m_state = 100;
break;
}
case 100:
{
m_state = 110;
break;
}
case 110: //move arm to place in good position for moving
{
grasp_left_hand();
choose_task_arm(3, LEFT_ARM);
if (moveArm(LEFT_ARM) == true)
m_state = 120;
break;
}
case 120:
{
my->setWheelVelocity(-1.4,-1.4);
choose_task_arm(5, LEFT_ARM);
if (moveArm(LEFT_ARM) == true)
{
Robot_speed = Change_Robot_speed;
m_state = 130;
sleep(1);
}
break;
}
case 130: //move to the object
{
// Robot_speed = 1;
if (goTo(m_relayFrontTrash, 0) == true)
{
m_state = 140;
}
break;
}
case 140:
{
if (goTo(m_relayFrontTable_reset, 0) == true)
{
sleep(2);
m_state = 150;
}
break;
}
case 990: //move to the object
{
// Robot_speed = 1;
break;
}
case 150: //Optional case !!!
{
Robot_speed = Change_Robot_speed;
if (m_pointedtrash=="recycle")
{
if (goTo(m_RecycleFront, 0) == true) m_state = 160;
}
else if (m_pointedtrash=="burnable")
{
if (goTo(m_BurnableFront, 0) == true) m_state = 160;
}
else if (m_pointedtrash=="unburnable")
{
if (goTo(m_UnburnableFront, 0) == true) m_state = 160;
}
break;
}
case 160: //preparation of the arm for grasp
{
recognizeObjectPosition(m_Trash_togo, m_pointedtrash);
if (goTo(m_Trash_togo, 50) == true)
{
m_state = 170;
}
break;
}
case 170: //preparation of the arm for grasp
{
choose_task_arm(1, LEFT_ARM);
if (moveArm(LEFT_ARM) == true) m_state = 180;
break;
}
case 180:
{
Robot_speed = 1;
if (goTo(m_Trash_togo, 60) == true) m_state = 190;
break;
}
case 190: //move arm to grasp the object
{
choose_task_arm(3, LEFT_ARM);
if (moveArm(LEFT_ARM) == true)
{
m_state = 200;
release_left_hand();
}
break;
}
case 200:
{
my->setWheelVelocity(-2.5,-2.5);
choose_task_arm(2, LEFT_ARM);
if (moveArm(LEFT_ARM) == true)
{
Robot_speed = Change_Robot_speed;
m_state = 210;
}
break;
}
case 210: //move to the object
{
Robot_speed = Change_Robot_speed;
if (goTo(m_relayFrontTrash, 0) == true) m_state = 220;
break;
}
case 220: //preparation of the arm for grasp
{
choose_task_arm(5, LEFT_ARM);
if (moveArm(LEFT_ARM) == true) m_state = 230;
break;
}
case 230: //move to the object
{
Robot_speed = Change_Robot_speed;
if (goTo(m_relayFrontTable, 0) == true)
{
cycle = cycle-1;
m_state = 40;
broadcastMsg("Task_finished");
m_pointedtrash = "";
m_pointedObject = "";
m_state = 0;
}
break;
}
}
return m_onActionReturn;
}
double DemoRobotController::onAction(ActionEvent &evt)
{
switch(m_state) {
case 0: {
break;
}
case 1: {
this->stopRobotMove();
break;
}
case 50: { // detour: rotate toward relay point 1
if(evt.time() >= m_time) {
this->stopRobotMove();
double l_moveTime = rotateTowardObj(m_relayPoint1);
m_time = l_moveTime+evt.time();
m_state = 60;
}
break;
}
case 60: { // detour: go toward relay point 1
if(evt.time() >= m_time) {
this->stopRobotMove();
double l_moveTime = goToObj(m_relayPoint1, 0.0);
m_time = l_moveTime+evt.time();
m_state = 70;
}
break;
}
case 70: { // rotate toward the position in front of trash
if(evt.time() >= m_time) {
this->stopRobotMove();
double l_moveTime = rotateTowardObj(m_frontDesk1);
m_time = l_moveTime+evt.time();
m_state = 80;
}
break;
}
case 80: { // go toward the position in front of trash
if(evt.time() >= m_time) {
this->stopRobotMove();
double l_moveTime = goToObj(m_frontDesk1, 0.0);
m_time = l_moveTime+evt.time();
m_state = 90;
}
break;
}
case 90: { // rotate toward the trash
if(evt.time() >= m_time) {
this->stopRobotMove();
Vector3d l_tpos;
if(m_task == 1)
this->recognizeObjectPosition(l_tpos, m_trashName1);
else
this->recognizeObjectPosition(l_tpos, m_trashName2);
double l_moveTime = rotateTowardObj(l_tpos);
m_time = l_moveTime+evt.time();
m_state = 100;
}
break;
}
case 100: { // prepare the robot arm to grasping the trash
if(evt.time() >= m_time) {
this->stopRobotMove();
this->neutralizeArms(evt.time());
m_state = 105;
}
break;
}
case 105: { // fix robot direction for grasping
if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
if(evt.time() >= m_time1 && evt.time() >= m_time4) {
Vector3d l_tpos;
if(m_task == 1)
this->recognizeObjectPosition(l_tpos, m_trashName1);
else
this->recognizeObjectPosition(l_tpos, m_trashName2);
double l_moveTime = rotateTowardObj(l_tpos);
m_time = l_moveTime+evt.time();
m_state = 110;
}
break;
}
case 110: { // approach to the trash
if(evt.time() >= m_time) {
this->stopRobotMove();
Vector3d l_tpos;
if(m_task == 1)
this->recognizeObjectPosition(l_tpos, m_trashName1);
else
this->recognizeObjectPosition(l_tpos, m_trashName2);
double l_moveTime = goToObj(l_tpos, 30.0);
m_time = l_moveTime+evt.time();
m_state = 120;
}
break;
}
case 120: { // try to grasp trash
if(evt.time() >= m_time) {
this->stopRobotMove();
Vector3d l_tpos;
if(m_task == 1)
this->recognizeObjectPosition(l_tpos, m_trashName1);
else
this->recognizeObjectPosition(l_tpos, m_trashName2);
double l_moveTime = goGraspingObject(l_tpos);
m_time = l_moveTime+evt.time();
m_state = 125;
}
break;
}
case 125: {
if(evt.time() >= m_time) {
m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
this->neutralizeArms(evt.time());
m_state = 130;
}
break;
}
case 130: {
if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
if(evt.time() >= m_time1 && evt.time() >= m_time4) {
m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity);
m_time = 20./m_movingSpeed + evt.time();
m_state = 150;
}
break;
}
case 150: {
if(evt.time() >= m_time) {
this->stopRobotMove();
double l_moveTime;
if(m_task == 1)
l_moveTime = rotateTowardObj(m_frontTrashBox1);
else
l_moveTime = rotateTowardObj(m_frontTrashBox2);
m_time = l_moveTime + evt.time();
m_state = 160;
}
break;
}
case 160: {
if(evt.time() >= m_time) {
this->stopRobotMove();
double l_moveTime;
if(m_task == 1)
l_moveTime = goToObj(m_frontTrashBox1,0.0);
else
l_moveTime = goToObj(m_frontTrashBox2,0.0);
m_time = l_moveTime + evt.time();
m_state = 161;
}
break;
}
case 161: {
if(evt.time() >= m_time) {
this->stopRobotMove();
this->prepareThrowing(evt.time());
m_state = 165;
}
break;
}
case 165: {
if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
if(evt.time() >= m_time1 && evt.time() >= m_time4) {
Vector3d l_tpos;
if(m_task == 1)
this->recognizeObjectPosition(l_tpos, m_trashBoxName1);
else
this->recognizeObjectPosition(l_tpos, m_trashBoxName2);
double l_moveTime = rotateTowardObj(l_tpos);
m_time = l_moveTime + evt.time();
m_state = 170;
}
break;
}
case 170: {
if(evt.time() >= m_time) {
this->stopRobotMove();
Vector3d l_tpos;
if(m_task == 1)
this->recognizeObjectPosition(l_tpos, m_trashBoxName1);
else
this->recognizeObjectPosition(l_tpos, m_trashBoxName2);
double l_moveTime = goToObj(l_tpos, 50.0);
m_time = l_moveTime + evt.time();
m_state = 180;
}
break;
}
case 180: {
if(evt.time() >= m_time) {
this->stopRobotMove();
Vector3d l_tpos;
if(m_task == 1)
this->recognizeObjectPosition(l_tpos, m_trashBoxName1);
else
this->recognizeObjectPosition(l_tpos, m_trashBoxName2);
double l_moveTime = rotateTowardObj(l_tpos);
m_time = l_moveTime + evt.time();
m_state = 200;
}
break;
}
case 200: { // throw trash and get back a bit
if(evt.time() >= m_time) {
this->stopRobotMove();
this->throwTrash();
sleep(1);
m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity);
m_time = 50.0/m_movingSpeed + evt.time();
m_state = 225;
}
break;
}
case 225: { // recover robot arms
if(evt.time() >= m_time) {
this->stopRobotMove();
this->neutralizeArms(evt.time());
m_state = 240;
}
break;
}
case 240: { // go next
if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
if(evt.time() >= m_time1 && evt.time() >= m_time4) {
this->stopRobotMove();
broadcastMsg("Task_finished");
//broadcastMsg("Give_up");
m_state = 0;
}
break;
}
}
return refreshRateOnAction;
}
