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;
}
