Beispiel #1
0
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
}
Beispiel #2
0
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);
	}
}
Beispiel #3
0
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);
}
Beispiel #4
0
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);
	}
}
Beispiel #5
0
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);
	}
}
Beispiel #6
0
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);
	}
}
Beispiel #7
0
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;
}
Beispiel #9
0
  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;
  }
Beispiel #10
0
					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);
						}
					}
Beispiel #11
0
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);
    }
  }
}
Beispiel #12
0
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");
}
Beispiel #13
0
//查看发送历史消息
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);
	}
}
Beispiel #14
0
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;
}