static void checkip_robots(struct mg_connection *conn) {
const char* remote_ip = conn->remote_ip;
send_headers(conn);
mg_printf_data(conn, robots);
updateRobot();
log_msg(daemon_mode, "robots.txt sent to %s", remote_ip);
}
void moveRobot(){
updateRobot();
updateRobotNew();
calcPos();
calcPosNew();
PIDcontrolNew();
if(Robot.newCom){
Stop(&motorL, &motorR);
Robot.errSumL = 0;
Robot.errSumR = 0;
counter++;
}
else{
if((Robot.dirL == STOP && Robot.dirR == STOP)){ //|| (time > Robot.time)
Stop(&motorR,&motorL);
}
if(Robot.dirR == FORWARD){
MoveRF(&motorR);
}
else if(Robot.dirR == BACKWARD){
MoveRB(&motorR);
}
if(Robot.dirL == FORWARD){
MoveLF(&motorL);
}
else if(Robot.dirL == BACKWARD){
MoveLB(&motorL);
}
}
if(counter > 2){
Robot.newCom = false;
counter = 0;
}
}
task main()
{
nxtState currentState;
initialize(currentState);
while(true){
updateInput(currentState);
updateRobot(currentState);
showDiagnostics(currentState);
}
}
void VisionModule::updateVisionRobot() {
portals::Message<messages::VisionRobot> robot_data(0);
messages::Robot* red1 = robot_data.get()->mutable_red1();
updateRobot(red1, vision->red1);
messages::Robot* red2 = robot_data.get()->mutable_red2();
updateRobot(red2, vision->red2);
messages::Robot* red3 = robot_data.get()->mutable_red3();
updateRobot(red3, vision->red3);
messages::Robot* navy1 = robot_data.get()->mutable_navy1();
updateRobot(navy1, vision->navy1);
messages::Robot* navy2 = robot_data.get()->mutable_navy2();
updateRobot(navy2, vision->navy2);
messages::Robot* navy3 = robot_data.get()->mutable_navy3();
updateRobot(navy3, vision->navy3);
vision_robot.setMessage(robot_data);
}
int main(int argc, char** argv){
ros::init(argc, argv, "humanMonitor");
ros::NodeHandle node;
HumanReader humanRd(node);
double far = 2.5;
double close = 1.3;
double distBodies = 0.0;
double distLHandToGripper = 0.0;
double distRHandToGripper = 0.0;
tf::TransformListener listener;
int niut_TORSO = 3;
int niut_LEFT_HAND = 9;
int niut_RIGHT_HAND = 15;
std::string humanTorso = "torso";
std::string humanLHand = "l_hand";
std::string humanRHand = "r_hand";
std::string robotTorso = "torso_lift_link";
std::string robotLGripper = "l_gripper_l_finger_link";
std::string robotRGripper = "r_gripper_l_finger_link";
humanMonitor::niut_JOINT_STR rHandJoint;
humanMonitor::niut_JOINT_STR lHandJoint;
humanMonitor::niut_JOINT_STR torsoJoint;
humanMonitor::niut_JOINT_STR rHandJointW;
humanMonitor::niut_JOINT_STR lHandJointW;
humanMonitor::niut_JOINT_STR torsoJointW;
double kinectPos[6];
//TODO: Move this in a service
kinectPos[0] = 7.25;
kinectPos[1] = 6.55;
kinectPos[2] = 2.2;
kinectPos[3] = 0;
kinectPos[4] = 0.84;
kinectPos[5] = -1.57;
while( node.ok() ){
ros::spinOnce();
// For now we focus on human with trackedId = 0.
if(humanRd.isPresent()){
std::cout << "[Fact] Human is present!" << std::endl;
//Get Human joints in kinect frame
rHandJoint = humanRd.m_LastConfig[0].skeleton.joint[niut_RIGHT_HAND];
lHandJoint = humanRd.m_LastConfig[0].skeleton.joint[niut_LEFT_HAND];
torsoJoint = humanRd.m_LastConfig[0].skeleton.joint[niut_TORSO];
//Get joints in same frame
if(rHandJoint.position.x != 0.0){
projectJoint(rHandJoint, kinectPos, rHandJointW);
m_HumanLastConfig[humanRHand] = rHandJointW;
}
if(lHandJoint.position.x != 0.0){
projectJoint(lHandJoint, kinectPos, lHandJointW);
m_HumanLastConfig[humanLHand] = lHandJointW;
}
if(torsoJoint.position.x != 0.0){
projectJoint(torsoJoint, kinectPos, torsoJointW);
m_HumanLastConfig[humanTorso] = torsoJointW;
}
m_HumanRBuffer.push_back(humanRd.m_LastTime, m_HumanLastConfig);
//Compute human motion:
long timeThreshold = pow(10,9); // 1sec
double distanceThreshold = 0.2; // 10 cms
if( isMoving(m_HumanRBuffer, humanTorso, timeThreshold, distanceThreshold, 2) ){
std::cout << "[Fact] Human is moving!" << std::endl;
}else{
if( isMoving(m_HumanRBuffer, humanRHand, timeThreshold, distanceThreshold*4/3, 3) ){
std::cout << "[Fact] Human right hand is moving" << std::endl;
}
if( isMoving(m_HumanRBuffer, humanLHand, timeThreshold, distanceThreshold*4/3, 3) ){
std::cout << "[Fact] Human left hand is moving" << std::endl;
}
}
updateRobot(listener);
//Compute relative distances (human / robot):
std::cout << "Human Torso x: " << torsoJointW.position.x << "y: " << torsoJointW.position.y << std::endl;
std::cout << "Robot Torso x: " << m_RobotLastConfig[robotTorso].position.x << "y: " << m_RobotLastConfig[robotTorso].position.y << std::endl;
if(torsoJoint.position.x != 0.0){
distBodies = dist2D(torsoJointW, m_RobotLastConfig[robotTorso]);
std::cout << "Dist human robot: " << distBodies << std::endl;
if( distBodies > far ){
std::cout << "[Fact] Human is far" << std::endl;
}else if(distBodies > close){
std::cout << "[Fact] Human is near" << std::endl;
//We compute the hand to gripper distance in NEAR case.
//Left gripper:
distLHandToGripper = dist3D(lHandJointW, m_RobotLastConfig[robotLGripper]);
distRHandToGripper = dist3D(rHandJointW, m_RobotLastConfig[robotLGripper]);
if( (distLHandToGripper < 0.5) || (distRHandToGripper < 0.5)){
std::cout << "[Fact] Danger! Human hand is close to left gripper!" << std::endl;
}
//Right gripper
distLHandToGripper = dist3D(lHandJointW, m_RobotLastConfig[robotRGripper]);
distRHandToGripper = dist3D(rHandJointW, m_RobotLastConfig[robotRGripper]);
if( (distLHandToGripper < 0.5) || (distRHandToGripper < 0.5)){
std::cout << "[Fact] Danger! Human hand is close to right gripper!" << std::endl;
}
}else
std::cout << "[Fact] Human is too close!" << std::endl;
}else
std::cout << "Human Torso not available " << std::endl;
}else
std::cout << "[Fact] Human is not present" << std::endl;
}
}
void DynamicRobotModel::processMessage( const std_msgs::String::ConstPtr& msg )
{
robot_description_ = msg->data;
updateRobot();
}
