/* * This file is part of robotreality * * Copyright(c) sschulz techfak.uni-bielefeld.de * http://opensource.cit-ec.de/projects/robotreality * * This file may be licensed under the terms of the * GNU General Public License Version 3 (the ``GPL''), * or (at your option) any later version. * * Software distributed under the License is distributed * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either * express or implied. See the GPL for the specific language * governing rights and limitations. * * You should have received a copy of the GPL along with this * program. If not, go to http://www.gnu.org/licenses/gpl.html * or write to the Free Software Foundation, Inc., * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. * * The development of this software was supported by the * Excellence Cluster EXC 277 Cognitive Interaction Technology. * The Excellence Cluster EXC 277 is a grant of the Deutsche * Forschungsgemeinschaft (DFG) in the context of the German * Excellence Initiative. * */ //Only build this when CMAKE detected Flobi support #ifndef CMAKE_XS2COMM_NOT_FOUND #include "DataOutputLCM.h" //slots for controller signals void device_found(int d){} #ifdef XS2_COMM_CONTOLLER_HAS_TIMESTAMP void device_has_new_data(int device, int reg, double d){} #else void device_has_new_data(int device, int reg){} #endif DataOutputLCM::DataOutputLCM(ConfigOptions *c) : DataOutput(c){ cfg = c; headtracker = NULL; fp = NULL; for(int i=0; iconnect_device_found_slot(&device_found); xs2ctrl->connect_device_has_new_data_slot(&device_has_new_data); xs2ctrl->init_communication(); usleep(500*1000); init_motorid_map(); set_output_active(cfg->data_output_active); lcm_server_connection_initialized = true; } void DataOutputLCM::start_file_logging(){ if (fp != NULL){ fclose(fp); } file_frame_id = 0; string filename = "/tmp/dump_live.csv"; printf("> dumping data to '%s'\n",filename.c_str()); fp = fopen(filename.c_str(), "w"); fprintf(fp, "# MOCAP_CSV LIVE\n"); fprintf(fp, "# \n"); fprintf(fp, "# WARNING: DO NOT IMPORT THIS DATA INTO BLENDER! USE XSC2_ANGLE_DUMPER OUTPUT!!!\n"); fprintf(fp, "#\n"); fprintf(fp, "# $FORMAT = 0.1\n"); fprintf(fp, "# $DESCRIPTION = auto gen\n"); fprintf(fp, "# $SOURCE = live tracking\n"); fprintf(fp, "# $AUDIO_FILE = none\n"); fprintf(fp, "# $AUDIO_SEEK = 0.0\n"); fprintf(fp, "# $CSV_DATE = 2012.02.02 - 02:02:02\n"); fprintf(fp, "# $FRAMERATE = 60.00\n"); fprintf(fp, "# $FRAMES = 10000\n"); fprintf(fp, "#\n"); fprintf(fp, "# $ENGINE{0} = /EYES/BOTH/UD\n"); fprintf(fp, "# $ENGINE{1} = /EYES/LEFT/BROW\n"); fprintf(fp, "# $ENGINE{2} = /EYES/LEFT/LID_LOWER\n"); fprintf(fp, "# $ENGINE{3} = /EYES/LEFT/LID_UPPER\n"); fprintf(fp, "# $ENGINE{4} = /EYES/LEFT/LR\n"); fprintf(fp, "# $ENGINE{5} = /EYES/RIGHT/BROW\n"); fprintf(fp, "# $ENGINE{6} = /EYES/RIGHT/LID_LOWER\n"); fprintf(fp, "# $ENGINE{7} = /EYES/RIGHT/LID_UPPER\n"); fprintf(fp, "# $ENGINE{8} = /EYES/RIGHT/LR\n"); fprintf(fp, "# $ENGINE{9} = /LIP/CENTER/LOWER\n"); fprintf(fp, "# $ENGINE{10} = /LIP/CENTER/UPPER\n"); fprintf(fp, "# $ENGINE{11} = /LIP/LEFT/LOWER\n"); fprintf(fp, "# $ENGINE{12} = /LIP/LEFT/UPPER\n"); fprintf(fp, "# $ENGINE{13} = /LIP/RIGHT/LOWER\n"); fprintf(fp, "# $ENGINE{14} = /LIP/RIGHT/UPPER\n"); fprintf(fp, "# $ENGINE{15} = /NECK/PAN\n"); fprintf(fp, "# $ENGINE{16} = /NECK/ROLL\n"); fprintf(fp, "# $ENGINE{17} = /NECK/TILT\n"); fprintf(fp, "#\n"); fprintf(fp, "# $LIP_RIGHT_UPPER = /LIP/RIGHT/UPPER\n"); fprintf(fp, "# $LIP_RIGHT_LOWER = /LIP/RIGHT/LOWER\n"); fprintf(fp, "# $LIP_CENTER_UPPER = /LIP/CENTER/UPPER\n"); fprintf(fp, "# $LIP_CENTER_LOWER = /LIP/CENTER/LOWER\n"); fprintf(fp, "# $LIP_LEFT_UPPER = /LIP/LEFT/UPPER\n"); fprintf(fp, "# $LIP_LEFT_LOWER = /LIP/LEFT/LOWER\n"); } void DataOutputLCM::start_output_thread(){ lcm_server_connection_initialized = false; printf("> starting lcm output thread...\n"); boost::thread *thread = new boost::thread(boost::bind( &DataOutputLCM::output_thread, this)); } void DataOutputLCM::output_thread(){ double fps = 60.0; while(!lcm_server_connection_initialized){ printf("> DataOutputLCM: waiting for lcm connection to be initialized...\n"); usleep(500000); } printf("> starting lcm mocap output thread\n"); boost::system_time timeout; timeout = boost::get_system_time() + boost::posix_time::milliseconds(1000/fps); while(1){ boost::thread::sleep(timeout); if (output_active){ //printf("LCM\n"); if (cfg->output_lcm_head){ if (headtracker != NULL){ xs2ctrl->set_register_translated_later(motor_id_map[NECK_PAN], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[NECK_PAN]); xs2ctrl->set_register_translated_later(motor_id_map[NECK_TILT], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[NECK_TILT]); xs2ctrl->set_register_translated_later(motor_id_map[NECK_ROLL], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[NECK_ROLL]); } } if (cfg->output_lcm_eyes){ xs2ctrl->set_register_translated_later(motor_id_map[EYE_LEFT_PAN], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[EYE_LEFT_PAN] * cfg->orientation_scale_factor.x); xs2ctrl->set_register_translated_later(motor_id_map[EYE_RIGHT_PAN], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[EYE_RIGHT_PAN] * cfg->orientation_scale_factor.x); xs2ctrl->set_register_translated_later(motor_id_map[EYES_TILT], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[EYES_TILT] * cfg->orientation_scale_factor.y); } if (cfg->output_lcm_eyelids){ xs2ctrl->set_register_translated_later(motor_id_map[EYE_LEFT_LID_LOWER], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[EYE_LEFT_LID_LOWER]); xs2ctrl->set_register_translated_later(motor_id_map[EYE_LEFT_LID_UPPER], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[EYE_LEFT_LID_UPPER]); xs2ctrl->set_register_translated_later(motor_id_map[EYE_RIGHT_LID_LOWER], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[EYE_RIGHT_LID_LOWER]); xs2ctrl->set_register_translated_later(motor_id_map[EYE_RIGHT_LID_UPPER], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[EYE_RIGHT_LID_UPPER]); } if (cfg->output_lcm_brows){ xs2ctrl->set_register_translated_later(motor_id_map[EYE_LEFT_BROW], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[EYE_LEFT_BROW]); xs2ctrl->set_register_translated_later(motor_id_map[EYE_RIGHT_BROW], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[EYE_RIGHT_BROW]); } if (cfg->output_lcm_mouth){ xs2ctrl->set_register_translated_later(motor_id_map[MOUTH_LEFT_UPPER], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[MOUTH_LEFT_UPPER]); xs2ctrl->set_register_translated_later(motor_id_map[MOUTH_LEFT_LOWER], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[MOUTH_LEFT_LOWER]); xs2ctrl->set_register_translated_later(motor_id_map[MOUTH_CENTER_UPPER], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[MOUTH_CENTER_UPPER]); xs2ctrl->set_register_translated_later(motor_id_map[MOUTH_CENTER_LOWER], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[MOUTH_CENTER_LOWER]); xs2ctrl->set_register_translated_later(motor_id_map[MOUTH_RIGHT_UPPER], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[MOUTH_RIGHT_UPPER]); xs2ctrl->set_register_translated_later(motor_id_map[MOUTH_RIGHT_LOWER], ASC2_PROTOCOL_REG_PID_TARGET, target_angle[MOUTH_RIGHT_LOWER]); } xs2ctrl->send_execute_request(PROTOCOL_DEVICEID_BROADCAST); } timeout = boost::get_system_time() + boost::posix_time::milliseconds(1000/fps); } } void DataOutputLCM::add_input_gui(){ //use dummy trackbar to get a new row: createTrackbar("LCM DUMMY0", "", &dummy_var, 1); //add buttons createButton("TX head",&button_wrapper_bool,&cfg->output_lcm_head,CV_CHECKBOX,cfg->output_lcm_head); createButton("TX eyes",&button_wrapper_bool,&cfg->output_lcm_eyes,CV_CHECKBOX,cfg->output_lcm_eyes); createButton("TX brows",&button_wrapper_bool,&cfg->output_lcm_brows,CV_CHECKBOX,cfg->output_lcm_brows); createButton("TX eyelids",&button_wrapper_bool,&cfg->output_lcm_eyelids,CV_CHECKBOX,cfg->output_lcm_eyelids); createButton("TX mouth",&button_wrapper_bool,&cfg->output_lcm_mouth,CV_CHECKBOX,cfg->output_lcm_mouth); createTrackbar("LCM DUMMY1", "", &dummy_var, 1); createButton("mirror mode",&button_wrapper_bool,&cfg->output_lcm_mirror_mode,CV_CHECKBOX,cfg->output_lcm_mirror_mode); // createButton("normalize view hist",&button_wrapper_bool,&cfg->normalize_resultview_histogram,CV_CHECKBOX,cfg->normalize_resultview_histogram); } void DataOutputLCM::set_engine_state(bool enabled){ //set datarate to 60hz for(int i=0; imotorid = %d\n",motor_id); if (enabled){ xs2ctrl->set_motor_enabled(motor_id, 1); xs2ctrl->set_register_execute(motor_id, ASC2_PROTOCOL_REG_PID_RAMP, 60); //60fps xs2ctrl->set_register_execute(motor_id, ASC2_PROTOCOL_REG_PID_CONTROLLER, ASC2_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK); }else{ xs2ctrl->set_register_execute(motor_id, ASC2_PROTOCOL_REG_PID_CONTROLLER, ASC2_PROTOCOL_PID_CONTROLLERTYPE_VELOCITY); xs2ctrl->set_motor_enabled(motor_id, 0); } } } void DataOutputLCM::set_output_active(bool state){ if (state != output_active){ //update state: if (state){ set_engine_state(true); start_file_logging(); }else{ set_engine_state(false); if (fp != NULL){ fclose(fp); fp = NULL; } } output_active = state; } } void DataOutputLCM::init_motorid_map(){ motor_id_map[NECK_PAN] = xs2ctrl->get_device_id_by_devicename("/NECK/PAN"); motor_id_map[NECK_TILT] = xs2ctrl->get_device_id_by_devicename("/NECK/TILT"); motor_id_map[NECK_ROLL] = xs2ctrl->get_device_id_by_devicename("/NECK/ROLL"); motor_id_map[EYES_TILT] = xs2ctrl->get_device_id_by_devicename("/EYES/BOTH/UD"); motor_id_map[EYE_LEFT_PAN] = xs2ctrl->get_device_id_by_devicename("/EYES/LEFT/LR"); motor_id_map[EYE_RIGHT_PAN] = xs2ctrl->get_device_id_by_devicename("/EYES/RIGHT/LR"); motor_id_map[EYE_RIGHT_LID_LOWER] = xs2ctrl->get_device_id_by_devicename("/EYES/RIGHT/LID_LOWER"); motor_id_map[EYE_RIGHT_LID_UPPER] = xs2ctrl->get_device_id_by_devicename("/EYES/RIGHT/LID_UPPER"); motor_id_map[EYE_LEFT_LID_LOWER] = xs2ctrl->get_device_id_by_devicename("/EYES/LEFT/LID_LOWER"); motor_id_map[EYE_LEFT_LID_UPPER] = xs2ctrl->get_device_id_by_devicename("/EYES/LEFT/LID_UPPER"); motor_id_map[EYE_LEFT_BROW] = xs2ctrl->get_device_id_by_devicename("/EYES/LEFT/BROW"); motor_id_map[EYE_RIGHT_BROW] = xs2ctrl->get_device_id_by_devicename("/EYES/RIGHT/BROW"); // #if 0 motor_id_map[MOUTH_LEFT_UPPER] = xs2ctrl->get_device_id_by_devicename("/LIP/LEFT/UPPER"); motor_id_map[MOUTH_LEFT_LOWER] = xs2ctrl->get_device_id_by_devicename("/LIP/LEFT/LOWER"); motor_id_map[MOUTH_CENTER_UPPER] = xs2ctrl->get_device_id_by_devicename("/LIP/CENTER/UPPER"); motor_id_map[MOUTH_CENTER_LOWER] = xs2ctrl->get_device_id_by_devicename("/LIP/CENTER/LOWER"); motor_id_map[MOUTH_RIGHT_UPPER] = xs2ctrl->get_device_id_by_devicename("/LIP/RIGHT/UPPER"); motor_id_map[MOUTH_RIGHT_LOWER] = xs2ctrl->get_device_id_by_devicename("/LIP/RIGHT/LOWER"); // #else // ///swap upper/lower (debug test) // motor_id_map[MOUTH_LEFT_LOWER] = xs2ctrl->get_device_id_by_devicename("/LIP/LEFT/UPPER"); // motor_id_map[MOUTH_LEFT_UPPER] = xs2ctrl->get_device_id_by_devicename("/LIP/LEFT/LOWER"); // motor_id_map[MOUTH_CENTER_LOWER] = xs2ctrl->get_device_id_by_devicename("/LIP/CENTER/UPPER"); // motor_id_map[MOUTH_CENTER_UPPER] = xs2ctrl->get_device_id_by_devicename("/LIP/CENTER/LOWER"); // motor_id_map[MOUTH_RIGHT_LOWER] = xs2ctrl->get_device_id_by_devicename("/LIP/RIGHT/UPPER"); // motor_id_map[MOUTH_RIGHT_UPPER] = xs2ctrl->get_device_id_by_devicename("/LIP/RIGHT/LOWER"); // #endif } DataOutputLCM::~DataOutputLCM(){ set_output_active(false); printf("> resetting xsc2 to zero positions\n"); xs2ctrl->reset_all_position_to_zero_deg(); delete xs2ctrl; } void DataOutputLCM::process_new_data(int f){ //dump to file if (fp != NULL){ fprintf(fp, "%d, ", file_frame_id); file_frame_id++; fprintf(fp, "%f, ", target_angle[EYES_TILT]); fprintf(fp, "%f, ", target_angle[EYE_LEFT_BROW]); fprintf(fp, "%f, ", target_angle[EYE_LEFT_LID_LOWER]); fprintf(fp, "%f, ", target_angle[EYE_LEFT_LID_UPPER]); fprintf(fp, "%f, ", target_angle[EYE_LEFT_PAN]); fprintf(fp, "%f, ", target_angle[EYE_RIGHT_BROW]); fprintf(fp, "%f, ", target_angle[EYE_RIGHT_LID_LOWER]); fprintf(fp, "%f, ", target_angle[EYE_RIGHT_LID_UPPER]); fprintf(fp, "%f, ", target_angle[EYE_RIGHT_PAN]); //mouth fprintf(fp, "%f, %f, ",target_angle[MOUTH_CENTER_LOWER],target_angle[MOUTH_CENTER_UPPER]); fprintf(fp, "%f, %f, ",target_angle[MOUTH_LEFT_LOWER],target_angle[MOUTH_LEFT_UPPER]); fprintf(fp, "%f, %f, ",target_angle[MOUTH_RIGHT_LOWER],target_angle[MOUTH_RIGHT_UPPER]); //head fprintf(fp, "%f, ", target_angle[NECK_PAN]); fprintf(fp, "%f, ", target_angle[NECK_ROLL]); fprintf(fp, "%f\n ", target_angle[NECK_TILT]); } } #endif