void RigidBody::_body_inout(int p_status, ObjectID p_instance, int p_body_shape, int p_local_shape) { bool body_in = p_status == 1; ObjectID objid = p_instance; Object *obj = ObjectDB::get_instance(objid); Node *node = obj ? obj->cast_to<Node>() : NULL; Map<ObjectID, BodyState>::Element *E = contact_monitor->body_map.find(objid); ERR_FAIL_COND(!body_in && !E); if (body_in) { if (!E) { E = contact_monitor->body_map.insert(objid, BodyState()); //E->get().rc=0; E->get().in_tree = node && node->is_inside_tree(); if (node) { node->connect(SceneStringNames::get_singleton()->tree_entered, this, SceneStringNames::get_singleton()->_body_enter_tree, make_binds(objid)); node->connect(SceneStringNames::get_singleton()->tree_exited, this, SceneStringNames::get_singleton()->_body_exit_tree, make_binds(objid)); if (E->get().in_tree) { emit_signal(SceneStringNames::get_singleton()->body_entered, node); } } } //E->get().rc++; if (node) E->get().shapes.insert(ShapePair(p_body_shape, p_local_shape)); if (E->get().in_tree) { emit_signal(SceneStringNames::get_singleton()->body_shape_entered, objid, node, p_body_shape, p_local_shape); } } else { //E->get().rc--; if (node) E->get().shapes.erase(ShapePair(p_body_shape, p_local_shape)); bool in_tree = E->get().in_tree; if (E->get().shapes.empty()) { if (node) { node->disconnect(SceneStringNames::get_singleton()->tree_entered, this, SceneStringNames::get_singleton()->_body_enter_tree); node->disconnect(SceneStringNames::get_singleton()->tree_exited, this, SceneStringNames::get_singleton()->_body_exit_tree); if (in_tree) emit_signal(SceneStringNames::get_singleton()->body_exited, obj); } contact_monitor->body_map.erase(E); } if (node && in_tree) { emit_signal(SceneStringNames::get_singleton()->body_shape_exited, objid, obj, p_body_shape, p_local_shape); } } }
void Area::_body_inout(int p_status,const RID& p_body, int p_instance, int p_body_shape,int p_area_shape) { bool body_in = p_status==PhysicsServer::AREA_BODY_ADDED; ObjectID objid=p_instance; Object *obj = ObjectDB::get_instance(objid); Node *node = obj ? obj->cast_to<Node>() : NULL; Map<ObjectID,BodyState>::Element *E=body_map.find(objid); ERR_FAIL_COND(!body_in && !E); locked=true; if (body_in) { if (!E) { E = body_map.insert(objid,BodyState()); E->get().rc=0; E->get().in_tree=node && node->is_inside_tree(); if (node) { node->connect(SceneStringNames::get_singleton()->enter_tree,this,SceneStringNames::get_singleton()->_body_enter_tree,make_binds(objid)); node->connect(SceneStringNames::get_singleton()->exit_tree,this,SceneStringNames::get_singleton()->_body_exit_tree,make_binds(objid)); if (E->get().in_tree) { emit_signal(SceneStringNames::get_singleton()->body_enter,node); } } } E->get().rc++; if (node) E->get().shapes.insert(ShapePair(p_body_shape,p_area_shape)); if (E->get().in_tree) { emit_signal(SceneStringNames::get_singleton()->body_enter_shape,objid,node,p_body_shape,p_area_shape); } } else { E->get().rc--; if (node) E->get().shapes.erase(ShapePair(p_body_shape,p_area_shape)); bool eraseit=false; if (E->get().rc==0) { if (node) { node->disconnect(SceneStringNames::get_singleton()->enter_tree,this,SceneStringNames::get_singleton()->_body_enter_tree); node->disconnect(SceneStringNames::get_singleton()->exit_tree,this,SceneStringNames::get_singleton()->_body_exit_tree); if (E->get().in_tree) emit_signal(SceneStringNames::get_singleton()->body_exit,obj); } eraseit=true; } if (node && E->get().in_tree) { emit_signal(SceneStringNames::get_singleton()->body_exit_shape,objid,obj,p_body_shape,p_area_shape); } if (eraseit) body_map.erase(E); } locked=false; }
int main(int argc, char **argv) { //So, for some odd reason involving the initialization of static non-copyable data, i can not get GUI to hold it's own RenderWindow //So i'm going to bite the bullet and just do it here in this class... :P gui.init(); //Create my BodyState tracker stateTracker=BodyState(); //Set up some ROS stuff ros::init(argc, argv, "mirror"); ros::NodeHandle n; //First my subscribers ros::Subscriber myoIMU_listener = n.subscribe("/myo/imu",1000, myoIMUCallBack); ros::Subscriber myoGesture_listener = n.subscribe("/myo/gesture",1000, myoGestureCallBack); ros::Subscriber myoOnboardGesture_listener = n.subscribe("/myo/onboardGesture",1000, myoOnboardGestureCallBack); ros::Subscriber left_eye_watcher = n.subscribe("/multisense/left/image_rect/compressed",1000, leftEyeCallBack); ros::Subscriber right_eye_watcher = n.subscribe("/multisense/right/image_rect/compressed",1000, rightEyeCallBack); ros::Subscriber joint_listener = n.subscribe("/ihmc_ros/valkyrie/output/joint_states", 1000, jointStateCallBack); ros::Subscriber transform_listener = n.subscribe("/tf", 1000, TFCallBack); ros::Subscriber people_listener = n.subscribe("/people", 1000, peopleCallBack); //Now my publishers ros::Publisher arm_controller = n.advertise<ihmc_msgs::ArmTrajectoryRosMessage>("/ihmc_ros/valkyrie/control/arm_trajectory", 1000); ros::Publisher neck_controller = n.advertise<ihmc_msgs::NeckTrajectoryRosMessage>("/ihmc_ros/valkyrie/control/neck_trajectory", 1000); ros::Publisher pelvis_height_controller = n.advertise<ihmc_msgs::PelvisHeightTrajectoryRosMessage>("/ihmc_ros/valkyrie/control/pelvis_height_trajectory", 1000); ros::Publisher chest_controller = n.advertise<ihmc_msgs::ChestTrajectoryRosMessage>("/ihmc_ros/valkyrie/control/chest_trajectory", 1000); ros::Publisher go_home_pub = n.advertise<ihmc_msgs::GoHomeRosMessage>("/ihmc_ros/valkyrie/control/go_home",1000); //Some SFML stufsf::RenderWindow windowf sf::RenderWindow window; if(gui.isOcculusOn){ sf::RenderWindow window(sf::VideoMode(2248, 544), "SFML works!"); sf::CircleShape shape(100.f); shape.setFillColor(sf::Color::Green); } //Now the primary loop ros::Rate loop_rate(6); ihmc_msgs::GoHomeRosMessage goHomeMsg; goHomeMsg.trajectory_time=3; goHomeMsg.unique_id=14; long int end, start; while (ros::ok()){ struct timeval tp; gettimeofday(&tp, NULL); long int start = tp.tv_sec * 1000 + tp.tv_usec / 1000; //std::cout<<tracking<<td::endl; //Some more SFML stuff if(GUIwindow.isOpen()){ //TODO:add in some human data gathering and print out human position and robot position data //Also, add a table for Myo stuff pollEventsGUI(); updateGUI(); } //std::cout<<lastLeftEye.height<<" "<<lastLeftEye.width<<" "<<lastLeftEye.encoding<<" "<<lastLeftEye.step<<" "<<lastLeftEye.data.size()<<" "<<1024*544*3<<std::endl; if(gui.isOcculusOn && window.isOpen() && lastLeftEye.data.size()!=0 && lastRightEye.data.size()!=0){ sf::Event event; while (window.pollEvent(event)) { if (event.type == sf::Event::Closed) window.close(); } sf::Uint8* pixels = new sf::Uint8[2248 * 544 * 4]; sf::Image image,imageL,imageR; sf::Uint8* Ldata=new sf::Uint8[(size_t)lastLeftEye.data.size()]; sf::Uint8* Rdata=new sf::Uint8[(size_t)lastRightEye.data.size()]; for(int i=0;i<(size_t)lastLeftEye.data.size();i++){ Ldata[i]=lastLeftEye.data[i]; } for(int i=0;i<(size_t)lastRightEye.data.size();i++){ Rdata[i]=lastRightEye.data[i]; } imageL.loadFromMemory(Ldata,(size_t)lastLeftEye.data.size()); imageR.loadFromMemory(Rdata,(size_t)lastRightEye.data.size()); delete Ldata; delete Rdata; if(imageL.getSize().x!=imageR.getSize().x || imageL.getSize().y!=imageR.getSize().y) std::cout<<"imageL:("<<imageL.getSize().x<<","<<imageL.getSize().y<<") imageR:("<<imageR.getSize().x<<","<<imageR.getSize().y<<")"<<std::endl; sf::Texture texture; int max_x=imageL.getSize().x; int max_y=imageL.getSize().y; for(int y = 0; y <max_y; y++) { for(int x = 0; x < max_x; x++) { pixels[(y*(2*max_x+200)+x)*4] = imageL.getPixelsPtr()[((max_y-y)*max_x+x)*4]; // R? pixels[(y*(2*max_x+200)+x)*4 + 1] = imageL.getPixelsPtr()[((max_y-y)*max_x+x)*4+1]; // G? pixels[(y*(2*max_x+200)+x)*4 + 2] = imageL.getPixelsPtr()[((max_y-y)*max_x+x)*4+2]; // B? pixels[(y*(2*max_x+200)+x)*4 + 3] = 255; // A? } for(int x=1024;x<1224;x++){ pixels[(y*(2*max_x+200)+x)*4] = 0; // R? pixels[(y*(2*max_x+200)+x)*4 + 1] = 0; // G? pixels[(y*(2*max_x+200)+x)*4 + 2] = 0; // B? pixels[(y*(2*max_x+200)+x)*4 + 3] = 255; // A? } for(int x = (max_x+200); x < (2*max_x+200); x++) { pixels[(y*(2*max_x+200)+x)*4] = imageR.getPixelsPtr()[((max_y-y)*max_x+x-(max_x+200))*4]; // R? pixels[(y*(2*max_x+200)+x)*4 + 1] = imageR.getPixelsPtr()[((max_y-y)*max_x+x-(max_x+200))*4+1]; // G? pixels[(y*(2*max_x+200)+x)*4 + 2] = imageR.getPixelsPtr()[((max_y-y)*max_x+x-(max_x+200))*4+2]; // B? pixels[(y*(2*max_x+200)+x)*4 + 3] = 255; // A? } } window.clear(); image.create(2248, 544, pixels); texture.create(2248, 544); texture.update(image); sf::Sprite sprite; sprite.setTexture(texture); window.draw(sprite); window.display(); delete pixels; } if(gui.goHomeCount==12){ goHomeMsg.body_part=0; goHomeMsg.robot_side=0; go_home_pub.publish(goHomeMsg); std::cout<<"just published goHome LEFT_ARM"<<std::endl; for(int i=0;i<6;i++){ ros::spinOnce(); loop_rate.sleep(); } goHomeMsg.robot_side=1; go_home_pub.publish(goHomeMsg); std::cout<<"just published goHome RIGHT_ARM"<<std::endl; for(int i=0;i<6;i++){ ros::spinOnce(); loop_rate.sleep(); } goHomeMsg.body_part=1; go_home_pub.publish(goHomeMsg); std::cout<<"just published goHome TORSO"<<std::endl; for(int i=0;i<6;i++){ ros::spinOnce(); loop_rate.sleep(); } goHomeMsg.body_part=2; go_home_pub.publish(goHomeMsg); std::cout<<"just published goHome PELVIS"<<std::endl; gui.startGoingHome=false; gui.goHomeCount--; ros::spinOnce(); loop_rate.sleep(); continue; /* moveSpeedOverRide=3; arm_controller.publish(moveArm(JRIGHT,stateTracker.getArmHomeAngles())); arm_controller.publish(moveArm(JLEFT,stateTracker.getArmHomeAngles())); chest_controller.publish(moveChest(stateTracker.getChestHomeAngles())); neck_controller.publish(moveHead(stateTracker.getNeckHomeAngles())); pelvis_height_controller.publish(movePelvisHeight(stateTracker.getPelvisHeightHomeAngles())); moveSpeedOverRide=-1;*/ } if(!more || justChanged || (gui.goHomeCount>0 && gui.goHomeCount<12)){ //std::cout<<gui.goHomeCount<<" "<<more<<" "<<justChanged<<std::endl; if(justChanged && more) justChanged=false; /* ihmc_msgs::GoHomeRosMessage msg; msg.trajectory_time=3; msg.unique_id=5; go_home_pub.publish(msg); msg.robot_side=1; go_home_pub.publish(msg);*/ if(gui.goHomeCount>0) gui.goHomeCount--; ros::spinOnce(); loop_rate.sleep(); continue; } std::vector<double> out; out.push_back((tp.tv_sec * 1000 + tp.tv_usec / 1000)-end);//oh wow, this is terrible programming practice... dang man if(end!=0) gui.record("*: ",out); //gui.gesture==0 acts as an unlock feature if(gui.isRightArmOn && gui.gesture==1) arm_controller.publish(moveArm(JRIGHT,stateTracker.getRightArmAngles())); else if(gui.isRightArmOn && gui.gesture!=1 && false){ moveSpeedOverRide=3; arm_controller.publish(moveArm(JRIGHT,stateTracker.getArmHomeAngles())); moveSpeedOverRide=-1; moveArm(JRIGHT,stateTracker.getRightArmAngles(),false); } if(gui.isLeftArmOn && gui.gesture==1) arm_controller.publish(moveArm(JLEFT,stateTracker.getLeftArmAngles())); else if(gui.isLeftArmOn && gui.gesture!=1 && false){ moveSpeedOverRide=3; arm_controller.publish(moveArm(JLEFT,stateTracker.getArmHomeAngles())); moveSpeedOverRide=-1; //moveArm(JLEFT,stateTracker.getLeftArmAngles(),false); } if(gui.isChestOn && gui.gesture==1) chest_controller.publish(moveChest(stateTracker.getChestAngles())); else if(gui.isChestOn && gui.gesture!=1 && false){ moveSpeedOverRide=3; chest_controller.publish(moveChest(stateTracker.getChestHomeAngles())); moveSpeedOverRide=-1; //moveChest(stateTracker.getChestAngles(),false); } if(gui.isHeadOn && gui.gesture==1) neck_controller.publish(moveHead(stateTracker.getNeckAngles())); else if(gui.isHeadOn && gui.gesture!=1 && false){ moveSpeedOverRide=3; neck_controller.publish(moveHead(stateTracker.getNeckHomeAngles())); moveSpeedOverRide=-1; //moveHead(stateTracker.getNeckAngles(),false); } if(gui.isPelvisOn && gui.gesture==1) pelvis_height_controller.publish(movePelvisHeight(stateTracker.getStandingHeight())); else if(gui.isPelvisOn && gui.gesture!=1 && false){ moveSpeedOverRide=3; pelvis_height_controller.publish(movePelvisHeight(stateTracker.getPelvisHeightHomeAngles())); moveSpeedOverRide=-1; //movePelvisHeight(stateTracker.getStandingHeight(),false); } ros::spinOnce(); gettimeofday(&tp, NULL); long int start2 = tp.tv_sec * 1000 + tp.tv_usec / 1000; loop_rate.sleep(); gettimeofday(&tp, NULL); end = tp.tv_sec * 1000 + tp.tv_usec / 1000; std::cout<<end-start2<<std::endl; } go_home_pub.publish(goHomeMsg); goHomeMsg.robot_side=1; go_home_pub.publish(goHomeMsg); return 0; }