// Send an event message with a boolean to component 'id'
bool Component::send(const char* const id, const int event, const bool value, SendData& sd)
{
bool val {};
Object* vv = sd.getValue(value);
if (vv != nullptr) {
Component* g = sd.getObject(this,id);
if (g != nullptr) val = g->event(event,vv);
}
return val;
}
// Send an event message with a double value to component 'id'
bool Component::send(const char* const id, const int event, const double value, SendData& sd)
{
bool val = false;
Object* vv = sd.getValue((LCreal)value);
if (vv != 0) {
Component* g = sd.getObject(this,id);
if (g != 0) val = g->event(event,vv);
}
return val;
}
//wrap SendData class
static void send_sql()
{
//I assume that values has 4 value ex: "0 0 0 0" "1 2 3 4"
//And if setting the other number of value , sendData will be failed.
//create header
std::string data = make_header(1, 1);
data += construct_select_statement(send_data_type);
data += "\n";
string db_response;
int ret = sd.Sender(data, db_response);
if (ret == -1) {
std::cerr << "Failed: sd.Sender" << std::endl;
return;
}
std::cout << "return data: " << db_response << std::endl;
std_msgs::String msg;
msg.data = db_response.c_str();
marker_publisher(msg);
}
//wrap SendData class
static void send_sql()
{
int sql_num = car_num + person_num + current_pose_position.size();
std::cout << "sqlnum : " << sql_num << std::endl;
//create header
std::string value = make_header(2, sql_num);
std::cout << "current_num=" << current_pose_position.size()
<< ", car_num=" << car_num << "(" << car_positions_array.size() << ")"
<< ",person_num=" << person_num << "(" << person_positions_array.size() << ")"
<< std::endl;
//get data of car and person recognizing
pthread_mutex_lock(&pose_lock_);
if(car_positions_array.size() > 0) {
for(size_t i = 0; i < car_positions_array.size(); i++) {
value += makeSendDataDetectedObj(car_positions_array[i], CAR_TOPIC_NAME);
}
}
car_positions_array.clear();
car_num = 0;
if(person_positions_array.size() > 0) {
for(size_t i = 0; i < person_positions_array.size(); i++) {
value += makeSendDataDetectedObj(person_positions_array[i], PERSON_TOPIC_NAME);
}
}
person_positions_array.clear();
person_num = 0;
// my_location
for(size_t i = 0; i < current_pose_position.size(); i++) {
std::string timestamp;
timestamp = getTimeStamp(current_pose_position[i].header.stamp.sec,current_pose_position[i].header.stamp.nsec);
value += pose_to_insert_statement(current_pose_position[i].pose, timestamp, OWN_TOPIC_NAME);
value += "\n";
}
current_pose_position.clear();
pthread_mutex_unlock(&pose_lock_);
#ifdef POS_DB_VERBOSE
std::cout << "val=" << value.substr(POS_DB_HEAD_LEN) << std::endl;
#endif /* POS_DB_VERBOSE */
std::string res;
int ret = sd.Sender(value, res, sql_num);
if (ret < 0) {
std::cerr << "Failed: sd.Sender" << std::endl;
return;
}
#ifdef POS_DB_VERBOSE
std::cout << "retrun message from DBserver : " << res << std::endl;
#endif /* POS_DB_VERBOSE */
return;
}
// Send an event message with an Object value to component 'id'
bool Component::send(const char* const id, const int event, Object* const value, SendData& sd)
{
// we don't check past values here, because it would be tedious and more overhead
// to go through each object and see if any data has changed. So we take a smaller
// hit just by sending the data through every time.
bool val = false;
if (value != nullptr) {
Component* g = sd.getObject(this,id);
if (g != nullptr) val = g->event(event, value);
}
return val;
}
void DispatcherPrivate::fire_channel(SYNCHRONIZATION_CHANNEL* channel)
{
channel->data->state->fired = true;
for (int i = 0; i < MAX_SEND_DATA; ++i)
{
SendData* sd = &channel->data->state->send[i];
if (sd->pin == NC)
continue;
switch (sd->mode)
{
case SendChannelModeNone:
break;
case SendChannelModeSet:
gpio_write(&sd->gpio, 1);
break;
case SendChannelModeToggle:
sd->last_write = 1 - sd->last_write;
gpio_write(&sd->gpio, sd->last_write);
break;
case SendChannelModeReset:
gpio_write(&sd->gpio, 0);
break;
case SendChannelModePulseUp:
gpio_write(&sd->gpio, 1);
sd->insert(us_ticker_read() + sd->pulse_length_us);
break;
case SendChannelModePulseDown:
gpio_write(&sd->gpio, 0);
sd->insert(us_ticker_read() + sd->pulse_length_us);
break;
}
}
if (channel->fired != NULL)
channel->fired(channel);
osSignalSet(_dispid, SIGNAL_CHANGED);
}
SendData::SendData(const SendData& c) : RPC2Request(Marshaller::METHOD_NSRPC2COMMUNICATION_BASICCOMMUNICATION__SENDDATA,c.getId())
{
*this=c;
}
//wrap SendData class
void* wrapSender(void *tsd){
//get values from sample_corner_point , convert latitude and longitude,
//and send database server.
std::vector<CARPOS> car_position_vector(global_cp_vector.size());
vector<CARPOS>::iterator cp_iterator;
//thread safe process for vector
pthread_mutex_lock( &mutex );
std::copy(global_cp_vector.begin(), global_cp_vector.end(), car_position_vector.begin());
pthread_mutex_unlock( &mutex );
cp_iterator = car_position_vector.begin();
while(cp_iterator != car_position_vector.end()){
double U = (cp_iterator.x1 + cp_iterator.x2)/2;
double V = (cp_iterator.y1 + cp_iterator.y2)/2;
//convert
sl.setOriginalValue(U,V,cp_iteartor.distance);
LOCATION ress = sl.cal();
//get axial rotation and movement vector from camera to velodyne.
ANGLE angle;
MoveVector mvvector;
axiMove am;
LOCATION velocoordinate = am.cal(ress,angle,mvvector);
/*
I got my GPS location too.it`s my_xloc,my_yloc.
*/
double my_xloc = 35.180188;
double my_yloc = 136.906565;
/*
process of conversion to absolute coodinate from relative coordinates.
yes, I did! next...
*/
LOCATION reccoord = am.cal(velecoordinate,angle);
calcoordinates cc;
RESULT res = cc.cal(ress.X,ress.Z,my_xloc,my_yloc);
//I assume that values has 4 value ex: "0,0,0,0" "1,2,3,4"
//And if setting the other number of value , sendData will be failed.
char values[100];
sprintf(values,"%f,%f,0,0",res.lat,res.log);
char dbn[100] = "prius_data_store";
char ct[100] = "latitude,logitude,type,id";
SendData sd;
sd.setData(values);
sd.setDBName(dbn);
sd.setColumnType(st);
sd.Sender();
cp_iterator++;
}
return NULL;
}
//wrap SendData class
void* wrapSender(void *tsd){
//get values from sample_corner_point , convert latitude and longitude,
//and send database server.
std::vector<CARPOS> car_position_vector(global_cp_vector.size());
std::vector<CARPOS>::iterator cp_iterator;
//thread safe process for vector
pthread_mutex_lock( &mutex );
std::copy(global_cp_vector.begin(), global_cp_vector.end(), car_position_vector.begin());
pthread_mutex_unlock( &mutex );
cp_iterator = car_position_vector.begin();
while(cp_iterator != car_position_vector.end()){
//middle of right-lower and left-upper
double U = (cp_iterator->x1 + cp_iterator->x2)/2;
double V = (cp_iterator->y1 + cp_iterator->y2)/2;
//convert
sl.setOriginalValue(U,V,cp_iterator->distance);
LOCATION ress = sl.cal();
//get axial rotation and movement vector from camera to velodyne.
ANGLE angle;
angle.thiX = 0;
angle.thiY = 0;
angle.thiZ = 0;
MoveVector mvvector;
mvvector.X = 0;
mvvector.Y = 0;
mvvector.Z = 0;
axiMove am;
LOCATION velocoordinate = am.cal(ress,angle,mvvector);
/*
I got my GPS location too.it`s my_xloc,my_yloc.
and I convert to plane rectangular coordinate from latitude and longitude.
*/
/*
our rectangular plane is 6 in Japan.
*/
double lat_plane = 36;//136.906565;
double lon_plane = 137.1;//35.180188;
//sample Longitude and Latitude 3513.1345669,N,13658.9971525,E
double my_xloc= 3513.1345669;
double my_yloc= 13658.9971525;
double my_zloc= 0;
/*
process of conversion to absolute coodinate from relative coordinates.
yes, I did! next...
*/
geo_pos_conv geo;
geo.set_plane(7);
geo.set_llh_nmea_degrees(my_xloc,my_yloc,my_zloc);
printf("geo : %f\t%f\n",geo.x(),geo.y());
LOCATION rescoord = am.cal(velocoordinate,angle);
rescoord.X = geo.x();
rescoord.Y = 0;
rescoord.Z = geo.y();
calcoordinates cc;
RESULT res = cc.cal(rescoord.X,rescoord.Z,lat_plane,lon_plane);
//I assume that values has 4 value ex: "0 0 0 0" "1 2 3 4"
//And if setting the other number of value , sendData will be failed.
char values[100];
sprintf(values,"%f %f 0 0",res.lat,res.log);
char dbn[100] = "prius_data_store";
char ct[100] = "latitude logitude type id";
SendData sd;
sd.setData(values);
sd.setDBName(dbn);
sd.setColumnType(ct);
sd.Sender();
cp_iterator++;
}
}
