void SessionProcess::exec(const Configuration& config,
boost::function<void (bool)> onReady)
{
#ifndef WT_WIN32
std::string parentPortOption = std::string("--parent-port=")
+ boost::lexical_cast<std::string>(acceptor_->local_endpoint().port());
const std::vector<std::string> &options = config.options();
const char **c_options = new const char*[options.size() + 2];
std::size_t i = 0;
for (; i < options.size(); ++i) {
c_options[i] = options[i].c_str();
}
c_options[i] = parentPortOption.c_str();
++i;
c_options[i] = 0;
pid_ = fork();
if (pid_ < 0) {
LOG_ERROR("failed to fork dedicated session process, error code: " << errno);
stop();
if (!onReady.empty()) {
onReady(false);
}
return;
} else if (pid_ == 0) {
#ifdef WT_THREADED
sigset_t mask;
sigfillset(&mask);
pthread_sigmask(SIG_UNBLOCK, &mask, 0);
#endif // WT_THREADED
execv(c_options[0], const_cast<char *const *>(c_options));
// An error occurred, this should not be reached
exit(1);
}
delete[] c_options;
#else // WT_WIN32
STARTUPINFOW startupInfo;
ZeroMemory(&startupInfo, sizeof(startupInfo));
startupInfo.cb = sizeof(startupInfo);
std::wstring commandLine = GetCommandLineW();
commandLine += L" --parent-port=" + boost::lexical_cast<std::wstring>(acceptor_->local_endpoint().port());
LPWSTR c_commandLine = new wchar_t[commandLine.size() + 1];
wcscpy(c_commandLine, commandLine.c_str());
if(!CreateProcessW(0, c_commandLine, 0, 0, true,
0, 0, 0, &startupInfo, &processInfo_)) {
LOG_ERROR("failed to start dedicated session process, error code: " << GetLastError());
stop();
if (!onReady.empty()) {
onReady(false);
}
}
delete c_commandLine;
#endif // WT_WIN32
}
void user::loadUnReadMessage( std::string type_string, std::string id_string, bool mark_read, boost::function<void(json::jobject)> callback )
{
IN_TASK_THREAD_WORKx(user::loadUnReadMessage, type_string, id_string, mark_read, callback);
if (!callback.empty())
{
json::jobject jobj;
get_parent_impl()->bizLocalConfig_->LoadMessage(type_string, id_string, -1, -1, jobj, kmrs_recv);
//查找jobj中的jid相关的showname 替换成现在内存中的showname
if (jobj["type"].get<std::string>() == "conversation")
{
for (int j = 0;j < jobj["data"].arr_size();j++)
{
std::string jid = jobj["data"][j]["jid"].get<std::string>();
ContactMap::iterator itvcard;
itvcard = get_parent_impl()->bizRoster_->syncget_VCard(jid);
std::string show_name;
if (itvcard != get_parent_impl()->bizRoster_->impl_->vcard_manager_.end())
{
show_name = itvcard->second.get_vcardinfo()["showname"].get<std::string>();
jobj["data"][j]["showname"] = show_name;
}
}
}
callback(jobj);
}
if (mark_read)
{
MarkMessageRead(type_string, id_string);
}
}
void SessionProcess::asyncExec(const Configuration &config,
boost::function<void (bool)> onReady)
{
asio::ip::tcp::endpoint endpoint(asio::ip::address_v4::loopback(), 0);
boost::system::error_code ec;
acceptor_->open(endpoint.protocol(), ec);
if (!ec)
acceptor_->set_option(asio::ip::tcp::acceptor::reuse_address(true), ec);
if (!ec)
acceptor_->bind(endpoint, ec);
if (!ec)
acceptor_->listen(0, ec);
#ifndef WT_WIN32
fcntl(acceptor_->native(), F_SETFD, FD_CLOEXEC);
#endif // !WT_WIN32
if (ec) {
LOG_ERROR("Couldn't create listening socket: " << ec.message());
if (!onReady.empty()) {
onReady(false);
return;
}
}
acceptor_->async_accept(*socket_,
boost::bind(&SessionProcess::acceptHandler, shared_from_this(),
asio::placeholders::error, onReady));
LOG_DEBUG("Listening to child process on port " << acceptor_->local_endpoint(ec).port());
exec(config, onReady);
}
void user::loadUnReadMessageCount(boost::function<void(json::jobject)> callback )
{
IN_TASK_THREAD_WORKx(user::loadUnReadMessageCount, callback);
if (!callback.empty())
{
json::jobject jobj;
get_parent_impl()->bizLocalConfig_->LoadUnreadMessageCount(jobj);
for (int j = 0;j < jobj["data"].arr_size();j++)
{
if (jobj["data"][j]["type"].get<std::string>() == "conversation")
{
std::string jid = jobj["data"][j]["jid"].get<std::string>();
ContactMap::iterator itvcard;
itvcard = get_parent_impl()->bizRoster_->syncget_VCard(jid);
std::string show_name;
if (itvcard != get_parent_impl()->bizRoster_->impl_->vcard_manager_.end())
{
show_name = itvcard->second.get_vcardinfo()["showname"].get<std::string>();
jobj["data"][j]["showname"] = show_name;
}
}
}
callback(jobj);
}
}
void user::loadOneNoticeMessage(std::string id_string,boost::function<void(json::jobject)> callback )
{
IN_TASK_THREAD_WORKx(user::loadOneNoticeMessage,id_string,callback);
if (!callback.empty())
{
json::jobject jobj = get_parent_impl()->bizLocalConfig_->LoadOneNoticeMessage(id_string);
callback(jobj);
}
}
void user::loadNoticeMessage(int offset, int count,boost::function<void(json::jobject)> callback )
{
IN_TASK_THREAD_WORKx(user::loadNoticeMessage,offset,count,callback);
if (!callback.empty())
{
json::jobject jobj = get_parent_impl()->bizLocalConfig_->LoadDescNoticeMessage(offset,count);
callback(jobj);
}
}
void user::loadMessage( std::string type_string, std::string id_string, int offset, int count, boost::function<void(json::jobject)> callback )
{
IN_TASK_THREAD_WORKx(user::loadMessage, type_string, id_string, offset, count, callback);
if (!callback.empty())
{
json::jobject jobj;
get_parent_impl()->bizLocalConfig_->LoadMessage(type_string, id_string, offset, count, jobj);
callback(jobj);
}
}
bool KDLArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
const double &timeout,
std::vector<double> &solution,
const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback,
const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback,
int &error_code)
{
if(!active_)
{
ROS_ERROR("kinematics not active");
error_code = kinematics::INACTIVE;
return false;
}
KDL::Frame pose_desired;
tf::PoseMsgToKDL(ik_pose, pose_desired);
//Do the IK
KDL::JntArray jnt_pos_in;
KDL::JntArray jnt_pos_out;
jnt_pos_in.resize(dimension_);
for(unsigned int i=0; i < dimension_; i++)
jnt_pos_in(i) = ik_seed_state[i];
if(!desired_pose_callback.empty())
desired_pose_callback(ik_pose,ik_seed_state,error_code);
if(error_code < 0)
{
ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision");
return false;
}
for(int i=0; i < max_search_iterations_; i++)
{
jnt_pos_in = getRandomConfiguration();
int ik_valid = ik_solver_pos_->CartToJnt(jnt_pos_in,pose_desired,jnt_pos_out);
if(ik_valid < 0)
continue;
std::vector<double> solution_local;
solution_local.resize(dimension_);
for(unsigned int j=0; j < dimension_; j++)
solution_local[j] = jnt_pos_out(j);
solution_callback(ik_pose,solution_local,error_code);
if(error_code == kinematics::SUCCESS)
{
solution = solution_local;
return true;
}
}
ROS_DEBUG("An IK that satisifes the constraints and is collision free could not be found");
if (error_code == 0)
error_code = kinematics::NO_IK_SOLUTION;
return false;
}
void initialize(boost::function<void(void)> aCallback, unsigned long aPeriodMS, HANDLE aTimerQueue = NULL)
{
stop();
if (aCallback.empty()) {
ANCHO_THROW(EInvalidArgument());
}
mCallback = aCallback;
mTimerQueue = aTimerQueue;
mPeriod = aPeriodMS;
}
void schedule(boost::function<void()> callback)
{
const char* const METHOD_NAME = "schedule";
if(!callback.empty())
{
callback();
}
else
{
printf("ERROR<%s::%s>: Bad parameters\n", CLASS_NAME, METHOD_NAME);
}
}
void SessionProcess::readPortHandler(const boost::system::error_code& err,
std::size_t transferred,
boost::function<void (bool)> onReady)
{
if (!err || err == asio::error::eof || err == asio::error::shut_down) {
socket_.reset();
buf_[transferred] = '\0';
port_ = atoi(buf_);
LOG_DEBUG("Child is listening on port " << port_);
if (!onReady.empty()) {
onReady(true);
}
}
}
void
AssociationHandlerBS::createDisassociation_ack(wns::service::dll::UnicastAddress destination,
wns::service::dll::UnicastAddress user,
bool preserve, std::string perMode,
boost::function<void()> callback) const
{
/** generate an empty PDU */
wns::ldk::CompoundPtr associationCompound =
wns::ldk::CompoundPtr(new wns::ldk::Compound(getFUN()->createCommandPool()));
/** activate and set MACg and command */
lte::macg::MACgCommand* macgCommand = dynamic_cast<lte::macg::MACgCommand*>(macgReader->activateCommand(associationCompound->getCommandPool()));
macgCommand->peer.source = layer2->getDLLAddress();
macgCommand->peer.dest = destination;
// activate RLC command
lte::rlc::RLCCommand* rlcCommand = dynamic_cast<lte::rlc::RLCCommand*>(rlcReader->activateCommand(associationCompound->getCommandPool()));
lte::controlplane::flowmanagement::IFlowSwitching::ControlPlaneFlowIDs _controlPlaneFlowIDs =
friends.flowManager->getControlPlaneFlowIDs(destination);
rlcCommand->peer.flowID = _controlPlaneFlowIDs[lte::helper::QoSClasses::DCCH()];
/** activate my command */
AssociationCommand* outgoingCommand = this->activateCommand(associationCompound->getCommandPool());
outgoingCommand->peer.myCompoundType = CompoundType::disassociation_ack();
outgoingCommand->peer.src = layer2->getDLLAddress();
outgoingCommand->peer.dst = destination;
outgoingCommand->peer.user = user;
outgoingCommand->peer.preserve = preserve;
outgoingCommand->peer.mode = perMode;
outgoingCommand->magic.bs = layer2->getDLLAddress();
outgoingCommand->magic.sender = this; // for debugging and assures
if (!callback.empty())
{
/** activate Phy User's command */
lte::macr::PhyCommand* outgoingPhyCommand = phyUser->activateCommand(associationCompound->getCommandPool());
outgoingPhyCommand->local.onAirCallback = callback;
}
/** send the compound */
if (getConnector()->hasAcceptor(associationCompound))
{
getConnector()->getAcceptor(associationCompound)->sendData(associationCompound);
MESSAGE_SINGLE(NORMAL, logger, "Sent "
<< outgoingCommand->peer.mode << separator << "disassociation_ack to "
<< layer2->getStationManager()->getStationByMAC(outgoingCommand->peer.dst)->getName());
}
else
assure(false, "Lower FU is not accepting scheduled disassociation_ack compound but is supposed to do so");
}
//查看发送历史消息
void user::loadpublishMessage(std::string id_string,boost::function<void(json::jobject)> callback )
{
IN_TASK_THREAD_WORKx(user::loadpublishMessage,id_string,callback);
if (!callback.empty())
{
json::jobject jobj;
get_parent_impl()->bizLocalConfig_->LoadpublishMessage(id_string,jobj);
jobj["myinfo"] = json::jobject();
std::string myJid = get_parent_impl()->bizClient_->jid().bare();
ContactMap::iterator itvcard = get_parent_impl()->bizRoster_->syncget_VCard(myJid);
if (itvcard != get_parent_impl()->bizRoster_->impl_->vcard_manager_.end())
{
jobj["myinfo"] = itvcard->second.get_vcardinfo().clone();
}
callback(jobj);
}
}
std::pair<int, S> runPortfolio(int numThreads,
boost::function<T()> driverFn,
boost::function<S()> threadFns[],
size_t stackSize,
bool optionWaitToJoin,
TimerStat& statWaitTime) {
boost::thread thread_driver;
boost::thread* threads = new boost::thread[numThreads];
S* threads_returnValue = new S[numThreads];
global_flag_done = false;
global_winner = -1;
for(int t = 0; t < numThreads; ++t) {
#if BOOST_HAS_THREAD_ATTR
boost::thread::attributes attrs;
if(stackSize > 0) {
attrs.set_stack_size(stackSize);
}
threads[t] =
boost::thread(attrs, boost::bind(runThread<S>, t, threadFns[t],
boost::ref(threads_returnValue[t]) ) );
#else /* BOOST_HAS_THREAD_ATTR */
if(stackSize > 0) {
throw OptionException("cannot specify a stack size for worker threads; requires CVC4 to be built with Boost thread library >= 1.50.0");
}
threads[t] =
boost::thread(boost::bind(runThread<S>, t, threadFns[t],
boost::ref(threads_returnValue[t]) ) );
#endif /* BOOST_HAS_THREAD_ATTR */
#if defined(BOOST_THREAD_PLATFORM_PTHREAD)
if(Chat.isOn()) {
void *stackaddr;
size_t stacksize;
pthread_attr_t attr;
pthread_getattr_np(threads[t].native_handle(), &attr);
pthread_attr_getstack(&attr, &stackaddr, &stacksize);
Chat() << "Created worker thread " << t << " with stack size " << stacksize << std::endl;
}
#endif
}
if(not driverFn.empty())
thread_driver = boost::thread(driverFn);
boost::unique_lock<boost::mutex> lock(mutex_main_wait);
while(global_flag_done == false) {
condition_var_main_wait.wait(lock);
}
statWaitTime.start();
if(not driverFn.empty()) {
thread_driver.interrupt();
thread_driver.join();
}
for(int t = 0; t < numThreads; ++t) {
if(optionWaitToJoin) {
threads[t].join();
}
}
std::pair<int, S> retval(global_winner, threads_returnValue[global_winner]);
delete[] threads;
delete[] threads_returnValue;
return retval;
}
int PR2ArmIKSolver::CartToJntSearch(const KDL::JntArray& q_in,
const KDL::Frame& p_in,
KDL::JntArray &q_out,
const double &timeout,
const double &max_consistency,
arm_navigation_msgs::ArmNavigationErrorCodes &error_code,
const boost::function<void(const KDL::JntArray&,const KDL::Frame&,arm_navigation_msgs::ArmNavigationErrorCodes &)> &desired_pose_callback,
const boost::function<void(const KDL::JntArray&,const KDL::Frame&,arm_navigation_msgs::ArmNavigationErrorCodes &)> &solution_callback)
{
Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
KDL::JntArray q_init = q_in;
double initial_guess = q_init(free_angle_);
ros::Time start_time = ros::Time::now();
double loop_time = 0;
int count = 0;
double max_limit = fmin(pr2_arm_ik_.solver_info_.limits[free_angle_].max_position, initial_guess+max_consistency);
double min_limit = fmax(pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, initial_guess-max_consistency);
ROS_DEBUG_STREAM("Initial guess " << initial_guess << " max " << max_consistency);
ROS_DEBUG_STREAM("Max limit " << max_limit << " " << pr2_arm_ik_.solver_info_.limits[free_angle_].max_position << " " << initial_guess+max_consistency);
ROS_DEBUG_STREAM("Min limit " << min_limit << " " << pr2_arm_ik_.solver_info_.limits[free_angle_].min_position << " " << initial_guess-max_consistency);
int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_angle_);
int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_angle_);
ROS_DEBUG("%f %f %f %d %d \n\n",initial_guess,pr2_arm_ik_.solver_info_.limits[free_angle_].max_position,pr2_arm_ik_.solver_info_.limits[free_angle_].min_position,num_positive_increments,num_negative_increments);
unsigned int testnum = 0;
if(!desired_pose_callback.empty())
desired_pose_callback(q_init,p_in,error_code);
if(error_code.val != error_code.SUCCESS)
{
return -1;
}
bool callback_check = true;
if(solution_callback.empty())
callback_check = false;
ros::WallTime s = ros::WallTime::now();
while(loop_time < timeout)
{
testnum++;
if(CartToJnt(q_init,p_in,q_out) > 0)
{
if(callback_check)
{
solution_callback(q_out,p_in,error_code);
if(error_code.val == error_code.SUCCESS)
{
ROS_DEBUG_STREAM("Difference is " << abs(q_in(free_angle_)-q_out(free_angle_)));
ROS_DEBUG_STREAM("Success with " << testnum << " in " << (ros::WallTime::now()-s));
return 1;
}
}
else
{
error_code.val = error_code.SUCCESS;
return 1;
}
}
if(!getCount(count,num_positive_increments,-num_negative_increments))
{
ROS_DEBUG_STREAM("Failure with " << testnum << " in " << (ros::WallTime::now()-s));
error_code.val = error_code.NO_IK_SOLUTION;
return -1;
}
q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
ROS_DEBUG("Redundancy search, index:%d, free angle value: %f",count,q_init(free_angle_));
loop_time = (ros::Time::now()-start_time).toSec();
}
if(loop_time >= timeout)
{
ROS_DEBUG("IK Timed out in %f seconds",timeout);
error_code.val = error_code.TIMED_OUT;
}
else
{
ROS_DEBUG("No IK solution was found");
error_code.val = error_code.NO_IK_SOLUTION;
}
return -1;
}
int InverseKinematicsSolver::CartToJntSearch(const KDL::JntArray& q_in,
const KDL::Frame& p_in,
KDL::JntArray &q_out,
const double &timeout,
motion_planning_msgs::ArmNavigationErrorCodes &error_code,
const boost::function<void(const KDL::JntArray&, const KDL::Frame&, motion_planning_msgs::ArmNavigationErrorCodes &)> &desired_pose_callback,
const boost::function<void(const KDL::JntArray&, const KDL::Frame&, motion_planning_msgs::ArmNavigationErrorCodes &)> &solution_callback)
{
KDL::JntArray q_init = q_in;
double initial_guess = q_init(_free_angle);
ros::Time start_time = ros::Time::now();
double loop_time = 0;
int count = 0;
int num_positive_increments = (int)((_solver_info.limits[_free_angle].max_position - initial_guess) / _search_discretization_angle);
int num_negative_increments = (int)((initial_guess - _solver_info.limits[_free_angle].min_position) / _search_discretization_angle);
ROS_DEBUG("%f %f %f %d %d \n\n", initial_guess, _solver_info.limits[_free_angle].max_position, _solver_info.limits[_free_angle].min_position, num_positive_increments, num_negative_increments);
if (!desired_pose_callback.empty()) {
desired_pose_callback(q_init, p_in, error_code);
}
if(error_code.val != error_code.SUCCESS) {
return -1;
}
bool callback_check = true;
if (solution_callback.empty()) {
callback_check = false;
}
while (loop_time < timeout) {
if (CartToJnt(q_init, p_in, q_out) > 0) {
if (callback_check) {
solution_callback(q_out,p_in,error_code);
if (error_code.val == error_code.SUCCESS) {
return 1;
}
} else {
error_code.val = error_code.SUCCESS;
return 1;
}
}
if (!getCount(count, num_positive_increments, -num_negative_increments)) {
error_code.val = error_code.NO_IK_SOLUTION;
return -1;
}
q_init(_free_angle) = initial_guess + _search_discretization_angle * count;
ROS_DEBUG("Redundancy search, index:%d, free angle value: %f", count, q_init(_free_angle));
loop_time = (ros::Time::now() - start_time).toSec();
}
if (loop_time >= timeout) {
ROS_DEBUG("IK Timed out in %f seconds",timeout);
error_code.val = error_code.TIMED_OUT;
} else {
ROS_DEBUG("No IK solution was found");
error_code.val = error_code.NO_IK_SOLUTION;
}
return -1;
}
int InverseKinematicsSolver::CartToJntSearch(const KDL::JntArray& q_in,
const KDL::Frame& p_in,
KDL::JntArray &q_out,
const double &timeout,
const double &max_consistency,
moveit_msgs::MoveItErrorCodes &error_code,
/*const boost::function<void(const KDL::JntArray&, const KDL::Frame&, moveit_msgs::MoveItErrorCodes &)> &desired_pose_callback,*/
/* OLD: const boost::function<void(const KDL::JntArray&, const KDL::Frame&, moveit_msgs::MoveItErrorCodes &)> &solution_callback)*/
const boost::function<void(const geometry_msgs::Pose&, const std::vector<double>&, moveit_msgs::MoveItErrorCodes &)> &solution_callback)
{
ROS_DEBUG("CartToJntSearch(consistency)\n");
KDL::JntArray q_init = q_in;
double initial_guess = q_init(_free_angle);
ros::Time start_time = ros::Time::now();
double loop_time = 0;
int count = 0;
double max_limit = fmin(_solver_info.limits[_free_angle].max_position, initial_guess + max_consistency);
double min_limit = fmax(_solver_info.limits[_free_angle].min_position, initial_guess - max_consistency);
ROS_DEBUG_STREAM("Initial guess " << initial_guess << " max " << max_consistency);
ROS_DEBUG_STREAM("Max limit " << max_limit << " " << _solver_info.limits[_free_angle].max_position << " " << initial_guess + max_consistency);
ROS_DEBUG_STREAM("Min limit " << min_limit << " " << _solver_info.limits[_free_angle].min_position << " " << initial_guess - max_consistency);
int num_positive_increments = (int)((max_limit - initial_guess) / _search_discretization_angle);
int num_negative_increments = (int)((initial_guess - min_limit) / _search_discretization_angle);
ROS_DEBUG("%f %f %f %d %d \n\n", initial_guess, _solver_info.limits[_free_angle].max_position, _solver_info.limits[_free_angle].min_position, num_positive_increments, num_negative_increments);
//if (!desired_pose_callback.empty()) {
// desired_pose_callback(q_init, p_in, error_code);
//}
//if(error_code.val != error_code.SUCCESS) {
// return -1;
//}
bool callback_check = true;
if (solution_callback.empty()) {
callback_check = false;
}
geometry_msgs::Pose ik_pose_msg;
tf::poseKDLToMsg(p_in, ik_pose_msg);
while (loop_time < timeout) {
if (CartToJnt(q_init, p_in, q_out) > 0) {
if (callback_check) {
std::vector<double> ik_solution(7,0.0);
for (int i = 0; i < q_out.rows(); ++i) {
ik_solution[i] = q_out(i);
}
ROS_DEBUG("Doing callback 2\n");
solution_callback(ik_pose_msg,ik_solution,error_code);
if (error_code.val == error_code.SUCCESS) {
ROS_DEBUG_STREAM("Difference is " << abs(q_in(_free_angle)-q_out(_free_angle)));
return 1;
}
ROS_DEBUG("Callback returned != SUCCESS\n");
} else {
error_code.val = error_code.SUCCESS;
return 1;
}
}
if (!getCount(count, num_positive_increments, -num_negative_increments)) {
ROS_DEBUG("!getCount 2, count %d\n", count);
error_code.val = error_code.NO_IK_SOLUTION;
return -1;
}
q_init(_free_angle) = initial_guess + _search_discretization_angle * count;
ROS_DEBUG("Redundancy search, index:%d, free angle value: %f", count, q_init(_free_angle));
loop_time = (ros::Time::now() - start_time).toSec();
}
if (loop_time >= timeout) {
ROS_DEBUG("IK Timed out in %f seconds",timeout);
error_code.val = error_code.TIMED_OUT;
} else {
ROS_DEBUG("No IK solution was found");
error_code.val = error_code.NO_IK_SOLUTION;
}
return -1;
}
