bool games_t::quit(int _pid, char* _pname) {
if(_pid < 0) return false;
if(_pid > MAX_PLAYER) return false;
mtx_all_games.lock();
if(all_players_state[_pid] == PS_UNDEF) {
mtx_all_games.unlock();
write_score((char*)_pname, scores[_pid]);
return true;
}
if(all_players_state[_pid] == PS_FREE ||
all_players_state[_pid] == PS_WIN1 ||
all_players_state[_pid] == PS_WIN2) {
all_players_state[_pid] = PS_UNDEF;
players.remove(_pid);
mtx_all_games.unlock();
write_score((char*)_pname, scores[_pid]);
return true;
}
if(all_players_state[_pid] == PS_IN_GAME) {
mtx_all_games.unlock();
int ngid = get_game(_pid);
if(ngid == -1) {
printf("quit ERROR pid %d without gid\n", _pid);
return true;
}
mtx_all_games.unlock();
all_games.add_lost_to_score(ngid, _pid);
all_games.app_lost(ngid, _pid);
mtx_all_games.lock();
}
if(all_players_state[_pid] == PS_WAIT_GAME) {
int ngid = get_game(_pid);
if(ngid == -1) {
printf("quit ERROR pid %d without gid\n", _pid);
return true;
}
mtx_all_games.unlock();
infos[ngid].player1_color = WHITE;
infos[ngid].turn = FREE_SPACE;
infos[ngid].player1_id = -1;
sprintf(infos[ngid].player1_name, "%s", (char*)"");
mtx_all_games.lock();
}
all_players_state[_pid] = PS_UNDEF;
players.remove(_pid);
mtx_all_games.unlock();
write_score((char*)_pname, scores[_pid]);
return true;
}
void LaserScan_callback(const sensor_msgs::LaserScan::ConstPtr& msg) {
/* Using input from the front laser scanner check for two things:
1. Whether there are not obstacles in front of the robot (close range)
2. To which direction should the robot face to drive without hitting an obstacle (mid-range)
*/
/*
Number of range measurements = 512
Angle of each sensor = about 0.35 degrees (0.00613592332229 radians)
Field of view = about 180 degrees (3.141592741 radians)
*/
// look for a clear corridor of 30 degrees and 1.5 meters ahead
int numPixels = 512;
float pixelAngle = 0.35; // in degrees
if(numPixels!=msg->ranges.size())
cout << "Warning: number of Hokuyo laser sensors different than usual (512)" << endl;
// check that we don't run into any thing
float safeDistance = 0.40; // 40cm
float safetyAngle = 120; // 120 degrees
int numClearPixelsPerSide = (safetyAngle/2)/pixelAngle;
if(!canAdvance(msg->ranges,numClearPixelsPerSide,safeDistance)) {
cout << "Stop, cannot advance" << endl;
dir_value_mutex.lock();
can_move = false;
dir_value_mutex.unlock();
} else {
dir_value_mutex.lock();
can_move = true;
dir_value_mutex.unlock();
}
// find corridor direction
int numCorridorPixels = 120; // 120*0.00613592332229*180/pi=42 degrees
float clearDistance = 1.5; // 1.5 meters ahead
float degreesToTurn = findClearCorridor(msg->ranges,numCorridorPixels,clearDistance,pixelAngle);
// negative means to turn right
dir_value_mutex.lock();
direction = degreesToTurn;
dir_value_mutex.unlock();
cout << "Degrees to turn = " << degreesToTurn << endl;
}
void OnConnect(const boost::system::error_code &ec)
{
if(ec)
{
global_stream_lock.lock();
std::cout << "OnConnect Error: " << ec << ".\n";
global_stream_lock.unlock();
}
else
{
global_stream_lock.lock();
std::cout << "Connected!" << ".\n";
global_stream_lock.unlock();
}
}
static void push_lock(void* c, const CLockLocation& locklocation, bool fTry)
{
if (lockstack.get() == NULL)
lockstack.reset(new LockStack);
if (fDebug) printf("Locking: %s\n", locklocation.ToString().c_str());
dd_mutex.lock();
(*lockstack).push_back(std::make_pair(c, locklocation));
if (!fTry) {
BOOST_FOREACH(const PAIRTYPE(void*, CLockLocation)& i, (*lockstack)) {
if (i.first == c) break;
std::pair<void*, void*> p1 = std::make_pair(i.first, c);
if (lockorders.count(p1))
continue;
lockorders[p1] = (*lockstack);
std::pair<void*, void*> p2 = std::make_pair(c, i.first);
if (lockorders.count(p2))
{
potential_deadlock_detected(p1, lockorders[p2], lockorders[p1]);
break;
}
}
}
void
reset()
{
m_mutex.lock();
m_indent = 0;
m_mutex.unlock();
}
void writeMessage(){
while( working_ ){
Message msg;
queueMutex.lock();
if( msgQueue.empty( ) ){
queueMutex.unlock();
wait_condition_.notify_one();
continue;
}
msg = msgQueue.front();
msgQueue.pop();
queueMutex.unlock();
IO_Sync *info;
{
boost::mutex::scoped_lock lock(map_mutex_);
if(sync_map_.find( msg.name() ) == sync_map_.end() ){
std::string outFileName;
char *name = new char[ 8]();
strncpy( name,msg.name(),sizeof(msg.name() ) );
// memcpy( name, msg.name(), sizeof( msg.name() ) );
outFileName = BINARY_DIR"/output_" + boost::lexical_cast< std::string >( name ) + ".txt";
sync_map_[name] = new IO_Sync(outFileName);
}
info = sync_map_[msg.name()];
}
info->write(msg);
}
}
void
cloud_cb (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr & cloud)
{
cld_mutex.lock ();
g_cloud = cloud;
cld_mutex.unlock ();
}
void App::broadcastRobotState() {
// Set up message
drc::robot_state_t msg_out;
msg_out.utime = bot_timestamp_now();
msg_out.pose.translation.x = 0; msg_out.pose.translation.y = 0; msg_out.pose.translation.z = 0;
msg_out.pose.rotation.w = 1; msg_out.pose.rotation.x = 0; msg_out.pose.rotation.y = 0; msg_out.pose.rotation.z = 0;
int n_joints = 15;
msg_out.joint_position.assign(n_joints , 0 );
msg_out.joint_velocity.assign(n_joints , 0 );
msg_out.joint_effort.assign(n_joints , 0 );
msg_out.num_joints = n_joints;
msg_out.joint_name = {"lwr_arm_0_joint", "lwr_arm_1_joint", "lwr_arm_2_joint", "lwr_arm_3_joint", "lwr_arm_4_joint", "lwr_arm_5_joint", "lwr_arm_6_joint", "sdh_knuckle_joint", "sdh_thumb_2_joint", "sdh_thumb_3_joint", "sdh_finger_12_joint", "sdh_finger_13_joint", "sdh_finger_22_joint", "sdh_finger_23_joint", "sdh_finger_21_joint"};
// Add individual joint state messages
int kuka_joints = 7; //kukaState_->joint_position.size();
int sdh_joints = 8; //sdhState_->joint_position.size();
mtx_.lock();
for (int i = 0; i < kuka_joints; i++) {
msg_out.joint_position[ i ] = kukaState_.joint_position[ i ];
msg_out.joint_velocity[ i ] = kukaState_.joint_velocity[ i ];
msg_out.joint_effort[ i ] = kukaState_.joint_effort[ i ];
}
for (int j = 0; j < sdh_joints; j++) {
msg_out.joint_position[ j+7 ] = sdhState_.joint_position[ j ];
msg_out.joint_velocity[ j+7 ] = sdhState_.joint_velocity[ j ];
msg_out.joint_effort[ j+7 ] = sdhState_.joint_effort[ j ];
}
mtx_.unlock();
lcm_->publish("EST_ROBOT_STATE", &msg_out);
}
int __stdcall on_sample(SampleStruct s) {
if (!outlet_)
return 0;
float sample[num_channels] = {
s.leftEye.gazeX,s.leftEye.gazeY,
s.rightEye.gazeX,s.rightEye.gazeY,
s.leftEye.diam,s.rightEye.diam,
s.leftEye.eyePositionX,s.leftEye.eyePositionY,s.leftEye.eyePositionZ,
s.rightEye.eyePositionX,s.rightEye.eyePositionY,s.rightEye.eyePositionZ,
s.planeNumber};
// calc sample age
long long tracker_time = s.timestamp;
iV_GetCurrentTimestamp(&tracker_time);
double age = (tracker_time - s.timestamp)/1000000.0;
// push into LSL
outlet_->push_sample(sample,lsl::local_clock()-age);
mtxx_.lock();
x_pos = (s.leftEye.gazeX+s.rightEye.gazeX)/2.0;
y_pos = (s.leftEye.gazeY+s.rightEye.gazeY)/2.0;
mtxx_.unlock();
return 1;
}
int get_messages_expired ()
{
m_mutex.lock ();
int expired = m_expired;
m_mutex.unlock ();
return expired;
}
void cloud_cb_ (const PointCloudT::ConstPtr &callback_cloud, PointCloudT::Ptr& cloud, bool* new_cloud_available_flag)
{
cloud_mutex.lock () ; // for not overwriting the point cloud from another thread
*cloud = *callback_cloud ;
*new_cloud_available_flag = true ;
cloud_mutex.unlock () ;
}
// spin, modifying the state to different values
void MyInfo::modifyThreadFunc(
robot_interaction::LockedRobotState* locked_state,
int* counter,
double offset)
{
bool go = true;
while(go)
{
double val = offset;
for (int loops = 0 ; loops < 100 ; ++loops)
{
val += 0.0001;
locked_state->modifyState(boost::bind(&MyInfo::modifyFunc,
this,
_1,
val));
}
cnt_lock_.lock();
go = !quit_;
++*counter;
cnt_lock_.unlock();
checkState(*locked_state);
val += 0.000001;
}
}
// spin, setting the state to different values
void MyInfo::setThreadFunc(
robot_interaction::LockedRobotState* locked_state,
int* counter,
double offset)
{
bool go = true;
while(go)
{
double val = offset;
for (int loops = 0 ; loops < 100 ; ++loops)
{
val += 0.0001;
robot_state::RobotState cp1(*locked_state->getState());
cp1.setVariablePosition(JOINT_A, val + 0.00001);
cp1.setVariablePosition(JOINT_C, val + 0.00002);
cp1.setVariablePosition(JOINT_F, val + 0.00003);
locked_state->setState(cp1);
}
cnt_lock_.lock();
go = !quit_;
++*counter;
cnt_lock_.unlock();
checkState(*locked_state);
val += 0.000001;
}
}
void CleanProtocolExclThread(std::string proto)
{
while(true)
{
UINT status = CallProtoService(proto.c_str(), PS_GETSTATUS, 0, 0);
if(status > ID_STATUS_OFFLINE)
break;
boost::this_thread::sleep(boost::posix_time::seconds(2));
}
std::list<HANDLE> contacts;
for(HANDLE hContact = db_find_first(proto.c_str()); hContact; hContact = db_find_next(hContact, proto.c_str()))
if(db_get_b(hContact, "CList", "NotOnList", 0) && db_get_b(hContact, pluginName, "Excluded", 0))
contacts.push_back(hContact);
boost::this_thread::sleep(boost::posix_time::seconds(5));
clean_mutex.lock();
std::list<HANDLE>::iterator end = contacts.end();
for(std::list<HANDLE>::iterator i = contacts.begin(); i != end; ++i)
{
LogSpamToFile(*i, _T("Deleted"));
HistoryLogFunc(*i, "Deleted");
CallService(MS_DB_CONTACT_DELETE, (WPARAM)*i, 0);
}
clean_mutex.unlock();
}
int main(void)
{
boost::shared_ptr<boost::asio::io_service> io_svc(
new boost::asio::io_service
);
boost::shared_ptr<boost::asio::io_service::work> worker(
new boost::asio::io_service::work(*io_svc)
);
global_stream_lock.lock();
std::cout << "The program will exit once all work has finished.\n";
global_stream_lock.unlock();
boost::thread_group threads;
for(int i=1; i<=2; i++)
threads.create_thread(boost::bind(&WorkerThread, io_svc, i));
io_svc->post(boost::bind(&ThrowAnException, io_svc, 1));
io_svc->post(boost::bind(&ThrowAnException, io_svc, 2));
io_svc->post(boost::bind(&ThrowAnException, io_svc, 3));
io_svc->post(boost::bind(&ThrowAnException, io_svc, 4));
io_svc->post(boost::bind(&ThrowAnException, io_svc, 5));
threads.join_all();
return 0;
}
/**
* @brief It copies a point cloud every time a new one arrives.
*
* @param callbackCloud Point cloud.
*/
void cloudCallback(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &callbackCloud) {
m.lock();
*cloud = *callbackCloud;
pcl::copyPointCloud(*callbackCloud, *cloud);
m.unlock();
newCloud = true;
}
/* a function that creates a ZMQ socket and bind an open port in the configured range
* @param start_port the start port range
* @param end_port the end port range
* @param ctx ZeroMQ socket context
* @param port_in_use an out parameter that returns the port address that is bind to a socket
* @param type the socket type. default is ZMQ_PULL
* @ret a socket that is created from the function - THIS HAS TO BE FREED AFTER USAGE!!!!!!
* @throws PrestoWarningException when there is no available open port in the configured range
*/
socket_t* CreateBindedSocket(int start_port, int end_port, context_t* ctx, int* port_in_use, int type) {
socket_t* sock = new socket_t(*ctx, type);
#ifdef ZMQ_LINGER
int linger_period = SOCK_LINGER_TIME;
sock->setsockopt(ZMQ_LINGER, &linger_period, sizeof(linger_period));
#endif
int num_port_cand = end_port - start_port + 1;
bool bind_succeed = false;
port_bind_mutex.lock();
for (int i = 0; i < num_port_cand; ++i){
cur_port_assigned = (cur_port_assigned >= start_port && cur_port_assigned <= end_port) ?
cur_port_assigned : start_port; // to make the value circulate after hitting the end_port
string endpoint = "tcp://*:"+int_to_string(cur_port_assigned);
try {
sock->bind(endpoint.c_str());
bind_succeed = true;
*port_in_use = (cur_port_assigned++);
break;
} catch(const std::exception& e) {
++cur_port_assigned;
continue;
}
}
port_bind_mutex.unlock();
if (bind_succeed == false) {
LOG_ERROR("CreateBindedSocket ZMQ - fail to bind a port (no available port)");
throw PrestoWarningException("There is no open port in the configured range");
}
return sock;
}
void GetTaskTime(EnvironmentBasePtr _penv, std::vector<dReal> &vsolutionA, std::vector<dReal> &vsolutionB, TrajectoryBasePtr ptraj){
//thread::id get_id();
std::cout << "this thread ::" << boost::this_thread::get_id() << std::endl;
PlannerBasePtr planner = RaveCreatePlanner(_penv,"birrt");
//std::vector<dReal> vinitialconfig,vgoalconfig;
ptraj = RaveCreateTrajectory(_penv,"");
PlannerBase::PlannerParametersPtr params(new PlannerBase::PlannerParameters());
RobotBasePtr probot = _penv->GetRobot(robotname);
params->_nMaxIterations = 4000; // max iterations before failure
GraphHandlePtr pgraph;
{
EnvironmentMutex::scoped_lock lock(_penv->GetMutex()); // lock environment
params->SetRobotActiveJoints(probot); // set planning configuration space to current active dofs
//initial config
probot->SetActiveDOFValues(vsolutionA);
params->vinitialconfig.resize(probot->GetActiveDOF());
probot->GetActiveDOFValues(params->vinitialconfig);
//goal config
probot->SetActiveDOFValues(vsolutionB);
params->vgoalconfig.resize(probot->GetActiveDOF());
probot->GetActiveDOFValues(params->vgoalconfig);
//setting limits
vector<dReal> vlower,vupper;
probot->GetActiveDOFLimits(vlower,vupper);
//RAVELOG_INFO("starting to plan\n");
if( !planner->InitPlan(probot,params) ) {
//return ptraj;
}
// create a new output trajectory
if( !planner->PlanPath(ptraj) ) {
RAVELOG_WARN("plan failed \n");
//return NULL;
}
}
////RAVELOG_INFO(ss.str());
if (!! ptraj){
//std::cout << ptraj->GetDuration() << std::endl;
writeTrajectory(ptraj);
}
std::cout << "this thread ::" << boost::this_thread::get_id() << " has completed its work" << std::endl;
mtx__.lock();
completed_thread = boost::this_thread::get_id();
mtx__.unlock();
//return ptraj;
}
int main( int argc, char * argv[] )
{
boost::shared_ptr< boost::asio::io_service > io_service(
new boost::asio::io_service
);
boost::shared_ptr< boost::asio::io_service::work > work(
new boost::asio::io_service::work( *io_service )
);
global_stream_lock.lock();
std::cout << "[" << boost::this_thread::get_id()
<< "] Press [return] to exit." << std::endl;
global_stream_lock.unlock();
boost::thread_group worker_threads;
for( int x = 0; x < 2; ++x )
{
worker_threads.create_thread( boost::bind( &WorkerThread, io_service ) );
}
boost::shared_ptr< boost::asio::deadline_timer > timer(
new boost::asio::deadline_timer( *io_service )
);
timer->expires_from_now( boost::posix_time::seconds( 5 ) );
timer->async_wait( boost::bind( &TimerHandler, _1, timer ) );
std::cin.get();
io_service->stop();
worker_threads.join_all();
return 0;
}
void writeTrajectory(TrajectoryBasePtr ptraj){
mtx_.lock();
stringstream ss;
if (!!(ptraj)){
if ((ptraj->GetDuration() < timer)){
timer = ptraj->GetDuration();
//std::cout << "Storing Trajectory with following info" << std::endl;
//std::cout << timer << std::endl;
ptraj->serialize(ss);
traj.open("traj.xml");
traj.clear();
traj << ss.str();
traj.close();
}
else {
//std::cout << "Ignoring Trajectory Files" << std::endl;
}
}
else {
//std::cout << "Planning Failed : trajectory empty" << std::endl;
}
mtx_.unlock();
}
void
image_callback (const boost::shared_ptr<openni_wrapper::Image>& image)
{
img_mutex.lock ();
g_image = image;
img_mutex.unlock ();
}
void writeLoop(socket_ptr sock, string_ptr prompt)
{
string inputMsg;
int i = 1;
for(;;)
{
inputMsg = *prompt;
mainMutex.lock();
if(!sendMessage.empty())
{
cerr << "Message sent:" << sendMessage <<":\n";
inputMsg += sendMessage;
sock->write_some(buffer(inputMsg, inputSize));
//sendMessage.clear();
if(i==10)
{
sendMessage.clear();
i=1;
}else
++i;
}
mainMutex.unlock();
if(inputMsg.find("exit") != string::npos)
return;
inputMsg.clear();
boost::this_thread::sleep( boost::posix_time::millisec(1000));
}
}
void appendFinishedTexture(float*** container, int index, float** texture)
{
mutex_lock.lock();
container[index] = texture;
threads_ready++;
mutex_lock.unlock();
}
void
disparity_image_msg_cb (const stereo_msgs::DisparityImageConstPtr& msg)
{
m.lock ();
disparity_image_msg = msg;
m.unlock ();
}
void
push()
{
m_mutex.lock();
m_indent++;
m_mutex.unlock();
}
void
cloud_msg_cb (const sensor_msgs::PointCloud2ConstPtr& msg)
{
m.lock ();
cloud_msg = msg;
m.unlock ();
}
int main(void)
{
boost::shared_ptr<boost::asio::io_service> io_svc(
new boost::asio::io_service
);
boost::shared_ptr<boost::asio::io_service::work> worker(
new boost::asio::io_service::work(*io_svc)
);
global_stream_lock.lock();
std::cout << "The program will exit once all work has finished.\n";
global_stream_lock.unlock();
boost::thread_group threads;
for(int i=1; i<=5; i++)
threads.create_thread(boost::bind(&WorkerThread, io_svc, i));
boost::this_thread::sleep(boost::posix_time::milliseconds(500));
io_svc->post(boost::bind(&Print, 1));
io_svc->post(boost::bind(&Print, 2));
io_svc->post(boost::bind(&Print, 3));
io_svc->post(boost::bind(&Print, 4));
io_svc->post(boost::bind(&Print, 5));
worker.reset();
threads.join_all();
return 0;
}
void PlatformCtrlNode::receiveOdo(const sensor_msgs::JointState& js)
{
mutex.lock();
//odometry msgs
nav_msgs::Odometry odom;
odom.header.stamp = ros::Time::now();
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
kin->execForwKin(js, odom, p);
topicPub_Odometry.publish(odom);
//odometry transform:
if(sendTransform)
{
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = odom.header.stamp;
odom_trans.header.frame_id = odom.header.frame_id;
odom_trans.child_frame_id = odom.child_frame_id;
odom_trans.transform.translation.x = odom.pose.pose.position.x;
odom_trans.transform.translation.y = odom.pose.pose.position.y;
odom_trans.transform.translation.z = odom.pose.pose.position.z;
odom_trans.transform.rotation = odom.pose.pose.orientation;
odom_broadcaster.sendTransform(odom_trans);
}
mutex.unlock();
}
void AutoTune::WorkWithParameters(int workerID, double pCrit, double nCrit, double epsilon)
{
LOG(INFO)<<"start with worker ID = "<<workerID;
CHECK_GE(workerID, 0);
CHECK_LE(workerID, this->workerNum);
//psoSolver solver(this->dim, this->trainPatches, this->lambda, pCrit,nCrit,epsilon);
vector<double> weights(dim);
weights[0] = pCrit;
weights[1] = nCrit;
weights[2] = epsilon;
weights[3] = 1.0-pCrit-nCrit-epsilon;
//solver.GetResult(weights);
double accuracy;
if(weights[3]<0)
accuracy = 0;
else
EvaluateResult(weights, accuracy);
//write back results.
myLock.lock();
CHECK_EQ(weights.size(), this->weightResults[workerID].size());
for (int i=0; i<dim; i++) {
this->weightResults[workerID][i] = weights[i];
}
this->accuracies[workerID] = accuracy;
myLock.unlock();
}
void writeLoop(socket_ptr sock, string_ptr prompt)
{
char inputBuf[inputSize] = {0};
for(;;)
{
//cin.getline(inputBuf, inputSize);
//inputMsg = *prompt + (string)inputBuf + '\n';
mainMutex.lock();
if(!sendMessage.empty())
{
sendMessage = *prompt + sendMessage;
cout << sendMessage << sendMessage.empty() << endl;
//sock->write_some(buffer(inputMsg, inputSize));
sock->write_some(buffer(sendMessage, inputSize));
}
if(sendMessage.find("exit") != string::npos){
mainMutex.unlock();
exit(1);
}
sendMessage.clear();
memset(inputBuf,0,inputSize);
mainMutex.unlock();
boost::this_thread::sleep( boost::posix_time::millisec(500));
}
}
