status_t MPlayer::start()
{
LOGV("MPlayer::start");
// Mutex::Autolock lock(mMutex);
if (mState == MP_STATE_PAUSED){
MPLAYER_DBG();
mState=MP_STATE_STARTED;
setStartState();
MPLAYER_DBG();
// return NO_ERROR;
}
if ( mplayerIsPlayEnd() == 1 ) {
MPLAYER_PRINTF("mplayerIsPlayEnd(), mplayerRestart\n");
mplayerRestart();
}
MPLAYER_DBG();
mState = MP_STATE_STARTED;
MPLAYER_PRINTF("wake up reader thread, mCondition.signal\n");
setMPlayerStart();
mCondition.signal(); // wake up prepare, pause loop
MPLAYER_DBG();
return NO_ERROR;
}
ConstraintModel& ScenarioCreate<ConstraintModel>::redo(
const id_type<ConstraintModel>& id,
const id_type<AbstractConstraintViewModel>& fullviewid,
StateModel& sst,
StateModel& est,
double ypos,
ScenarioModel& s)
{
auto constraint = new ConstraintModel {
id,
fullviewid,
ypos,
&s};
constraint->setStartState(sst.id());
constraint->setEndState(est.id());
s.addConstraint(constraint);
sst.setNextConstraint(id);
est.setPreviousConstraint(id);
const auto& sev = s.event(sst.eventId());
const auto& eev = s.event(est.eventId());
ConstraintModel::Algorithms::changeAllDurations(*constraint,
eev.date() - sev.date());
constraint->setStartDate(sev.date());
return *constraint;
}
void KoulesSetup::initialize(unsigned int numKoules, const std::string& plannerName,
const std::vector<double>& stateVec)
{
const ob::StateSpacePtr& space = getStateSpace();
space->setup();
// setup start state
ob::ScopedState<> start(space);
if (stateVec.size() == space->getDimension())
space->copyFromReals(start.get(), stateVec);
else
{
// Pick koule positions evenly radially distributed, but at a linearly
// increasing distance from the center. The ship's initial position is
// at the center. Initial velocities are 0.
std::vector<double> startVec(space->getDimension(), 0.);
double r, theta = boost::math::constants::pi<double>(), delta = 2.*theta / numKoules;
startVec[0] = .5 * sideLength;
startVec[1] = .5 * sideLength;
startVec[4] = .5 * delta;
for (unsigned int i = 0; i < numKoules; ++i, theta += delta)
{
r = .1 + i * .1 / numKoules;
startVec[4 * i + 5] = .5 * sideLength + r * cos(theta);
startVec[4 * i + 6] = .5 * sideLength + r * sin(theta);
}
space->copyFromReals(start.get(), startVec);
}
setStartState(start);
// set goal
setGoal(std::make_shared<KoulesGoal>(si_));
// set propagation step size
si_->setPropagationStepSize(propagationStepSize);
// set min/max propagation steps
si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
// set directed control sampler; when using the PDST planner, propagate as long as possible
bool isPDST = plannerName == "pdst";
const ompl::base::GoalPtr& goal = getGoal();
si_->setDirectedControlSamplerAllocator(
[&goal, isPDST](const ompl::control::SpaceInformation *si)
{
return KoulesDirectedControlSamplerAllocator(si, goal, isPDST);
});
// set planner
setPlanner(getConfiguredPlannerInstance(plannerName));
// set validity checker
setStateValidityChecker(std::make_shared<KoulesStateValidityChecker>(si_));
// set state propagator
setStatePropagator(std::make_shared<KoulesStatePropagator>(si_));
}
void DuplicateInterval::redo(const score::DocumentContext& ctx) const
{
m_cmdStart.redo(ctx);
m_cmdEnd.redo(ctx);
auto& root = m_path.find(ctx);
auto scenar = safe_cast<Scenario::ProcessModel*>(root.parent());
auto obj = score::marshall<DataStream>(root);
auto interval
= new Scenario::IntervalModel{DataStream::Deserializer{obj}, scenar};
score::IDocument::changeObjectId(*interval, m_createdId);
interval->setStartState(m_cmdStart.createdState());
interval->setEndState(m_cmdEnd.createdState());
SetPreviousInterval(scenar->states.at(m_cmdEnd.createdState()), *interval);
SetNextInterval(scenar->states.at(m_cmdStart.createdState()), *interval);
interval->setHeightPercentage(root.heightPercentage() + 0.1);
scenar->intervals.add(interval);
}
ConstraintModel& ScenarioCreate<ConstraintModel>::redo(
const Id<ConstraintModel>& id,
const Id<ConstraintViewModel>& fullviewid,
StateModel& sst,
StateModel& est,
double ypos,
ScenarioModel& s)
{
auto constraint = new ConstraintModel {
id,
fullviewid,
ypos,
&s};
constraint->setStartState(sst.id());
constraint->setEndState(est.id());
s.constraints.add(constraint);
sst.setNextConstraint(id);
est.setPreviousConstraint(id);
const auto& sev = s.event(sst.eventId());
const auto& eev = s.event(est.eventId());
const auto& tn = s.timeNode(eev.timeNode());
ConstraintDurations::Algorithms::changeAllDurations(*constraint,
eev.date() - sev.date());
constraint->setStartDate(sev.date());
if(tn.trigger()->active())
{
constraint->duration.setRigid(false);
const auto& dur = constraint->duration.defaultDuration();
constraint->duration.setMinDuration( TimeValue::fromMsecs(0.8 * dur.msec()));
constraint->duration.setMaxDuration( TimeValue::fromMsecs(1.2 * dur.msec()));
}
return *constraint;
}
void DFA::read(Source &r)
{
#undef pt
#define pt(a) //pr(a)
reset();
int v;
r >> v;
pt((" version = %d\n",v));
if (v != VERSION)
throw IOException("Bad version in DFA");
short sCnt;
short sState;
short tokenNameCount;
r >> sCnt >> sState >> tokenNameCount;
pt((" # states=%d, start=%d\n",sCnt,sState));
for (int i = 0; i < sCnt; i++) {
addState(i);
DFAState &s = getState(i);
pt((" reading state %d\n",i));
s.read(r);
}
setStartState(sState);
for (int i = 0; i < tokenNameCount; i++) {
String n;
r >> n;
tokenNames_.add(n);
pt((" token %d= %s\n",i,n.chars() ));
}
pt((" done reading\n"));
}
/*!
Constructor sets up initial strategy
*/
ManagerForward::ManagerForward(KBot *kbot) : RobotManager(kbot)
{
setStartState(knProgrammedMove1);
StrategyProgrammedMove* pMove1 = (StrategyProgrammedMove*)(m_lstStrategy[knProgrammedMove1]);
pMove1->setState(knProgrammedMove1);
StrategyProgrammedMove* pMove2 = (StrategyProgrammedMove*)(m_lstStrategy[knProgrammedMove2]);
pMove2->setState(knProgrammedMove2);
StrategyProgrammedMove* pMove3 = (StrategyProgrammedMove*)(m_lstStrategy[knProgrammedMove3]);
pMove3->setState(knProgrammedMove3);
((StrategyShoot*)(m_lstStrategy[knShoot]))->getNextStates().push_back(knProgrammedMove2);
((StrategyShoot*)(m_lstStrategy[knShoot]))->getNextStates().push_back(knProgrammedMove3);
// Ball positions:
// 1 2 3
// 4 5 6
// 7 8 9
// Forward gets 1 ball.
switch(kbot->getAutoPattern())
{
case 0: // Kick ball in position 9
printf("***************************************Forward pattern 0\n");
pMove1->addPointToPath(0,0);
pMove1->addPointToPath(0.0, 72.0-10.42-ROBOT_LENGTH-HALF_RAMP_WIDTH);
pMove1->addPointToPath(-5.95, 10.42);
pMove2->addPointToPath(0,0);
pMove2->addPointToPath(35.67,62.54); // Get out of the way
// No more moves; remove knProgrammedMove3 from the list.
((StrategyShoot*)(m_lstStrategy[knShoot]))->getNextStates().pop_back();
break;
case 1: // Kick ball in position 6
printf("***************************************Forward pattern 1\n");
pMove1->addPointToPath(0,0);
pMove1->addPointToPath(0.0,9.0*12-9.56-ROBOT_LENGTH-HALF_RAMP_WIDTH);
pMove1->addPointToPath(-7.26,9.56);
pMove2->addPointToPath(0,0);
pMove2->addPointToPath(21.77,28.68); // Get out of the way
// No more moves; remove knProgrammedMove3 from the list.
((StrategyShoot*)(m_lstStrategy[knShoot]))->getNextStates().pop_back();
break;
case 2: // Kick ball in position 3
printf("***************************************Forward pattern 2\n");
pMove1->addPointToPath(0,0);
pMove1->addPointToPath(0.0,12.0*12-7.92-ROBOT_LENGTH-HALF_RAMP_WIDTH);
pMove1->addPointToPath(-9.02,7.92);
// No more moves; remove knProgrammedMove2 & 3 from the list.
((StrategyShoot*)(m_lstStrategy[knShoot]))->getNextStates().pop_back();
((StrategyShoot*)(m_lstStrategy[knShoot]))->getNextStates().pop_back();
break;
case 3: // Kick ball in position 2
default:
printf("***************************************Forward pattern 3\n");
pMove1->addPointToPath(0,0);
pMove1->addPointToPath(0.0,12.0*12-10.42-ROBOT_LENGTH-HALF_RAMP_WIDTH);
pMove1->addPointToPath(-5.95,10.42);
pMove2->addPointToPath(0,0);
pMove2->addPointToPath(31.27,-17.84); // Get out of the way
// No more moves; remove knProgrammedMove3 from the list.
((StrategyShoot*)(m_lstStrategy[knShoot]))->getNextStates().pop_back();
}
reset();
// TODO: set parameters on programmed moves properly
}
bool MoveArm::makePlan(){
std::cout << "Invoking Kinematic Planner\n";
//lock();
ROS_INFO("Locked");
robot_srvs::PlanNames::request namesReq;
robot_srvs::PlanNames::response names;
ros::service::call("plan_joint_state_names", namesReq, names);
//unsigned int needparams = 0;
//for (unsigned int i = 0 ; i < names.get_names_size() && i < names.get_num_values_size() ; ++i) {
//std::cout << names.names[i] << " (" << names.num_values[i] << ")\n";
// needparams += names.num_values[i];
//}
//std::cout << "Need " << needparams << " Params\n";
robot_srvs::NamedKinematicPlanState::request req;
req.params.model_id = kinematicModel;
req.params.distance_metric = "L2Square";
req.threshold = 10e-06;
req.interpolate = 0;
req.times = 1;
//Get the pose of the robot:
tf::Stamped<tf::Pose> robotPose, globalPose;
robotPose.setIdentity();
robotPose.frame_id_ = "base_link";
robotPose.stamp_ = ros::Time();
try{
tf_.transformPose("map", robotPose, globalPose);
}
catch(tf::LookupException& ex) {
ROS_INFO("No Transform available Error\n");
}
catch(tf::ConnectivityException& ex) {
ROS_INFO("Connectivity Error\n");
}
catch(tf::ExtrapolationException& ex) {
ROS_INFO("Extrapolation Error\n");
}
ROS_INFO("Get the start state.");
//initializing full value state
req.start_state.set_names_size(names.get_names_size());
req.start_state.set_joints_size(names.get_names_size());
for (unsigned int i = 0 ; i < req.start_state.get_joints_size() ; ++i) {
req.start_state.names[i] = names.names[i];
//std::cout << req.start_state.names[i] << ": " << names.num_values[i] << std::endl;
req.start_state.joints[i].set_vals_size(names.num_values[i]);
if (names.names[i] == "base_joint") {
double yaw, pitch, roll;
globalPose.getBasis().getEulerZYX(yaw, pitch, roll);
std::cout << "Base: " << i << ", " << globalPose.getOrigin().getX() << ", " << globalPose.getOrigin().getY() << ", " << yaw << std::endl;
req.start_state.joints[i].vals[0] = globalPose.getOrigin().getX();
req.start_state.joints[i].vals[1] = globalPose.getOrigin().getY();
req.start_state.joints[i].vals[2] = yaw;
} else {
for (int k = 0 ; k < names.num_values[i]; k++) {
req.start_state.joints[i].vals[k] = 0;
}
}
}
// TODO: Adjust based on parameters for left vs right arms
// Fill out the start state from current arm configuration
setStartState(req);
// Filling out goal state from data in the goal message
ROS_INFO("Set the goal state.");
req.goal_state.set_names_size(goalMsg.get_configuration_size());
req.goal_state.set_joints_size(goalMsg.get_configuration_size());
for(unsigned int i = 0; i<goalMsg.get_configuration_size(); i++) {
req.goal_state.names[i] = goalMsg.configuration[i].name;
req.goal_state.joints[i].set_vals_size(1); //FIXME: multi-part joints?
req.goal_state.joints[i].vals[0] = goalMsg.configuration[i].position;
ROS_INFO("Joint: %s = %f", goalMsg.configuration[i].name.c_str(), goalMsg.configuration[i].position);
}
req.allowed_time = 10.0;
req.params.volumeMin.x = -1.0; req.params.volumeMin.y = -1.0; req.params.volumeMin.z = -1.0;
req.params.volumeMax.x = -1.0; req.params.volumeMax.y = -1.0; req.params.volumeMax.z = -1.0;
// Invoke kinematic motion planner
ROS_INFO("Running the service.");
ros::service::call("plan_kinematic_path_named", req, plan);
unsigned int nstates = plan.path.get_states_size();
// If all well, then there will be at least 2 states
bool foundPlan = (nstates > 0);
if (foundPlan) {
currentWaypoint = 0;
std::cout << "Obtained solution path with " << nstates << " states\n";
} else {
std::cout << "Service 'plan_kinematic_path_named' failed\n";
}
/*robot_msgs::DisplayKinematicPath dpath;
dpath.frame_id = "robot";
dpath.model_name = req.params.model_id;
dpath.start_state = req.start_state;
dpath.path = plan.path;
publish("display_kinematic_path", dpath);*/
unlock();
return foundPlan;
}
void DFA::readText(Source &r)
{
reset();
// TextReader r(path);
StringReader tk;
String lineStr;
// storage for strings parsed from line; moved out
// of main loop to minimize object construction/destruction
String stateA;
String stateB;
String symbol;
// keep track of what will become the start state
int startSt = -1;
while (!r.eof()) {
r >> lineStr;
// r.readLine(lineStr);
tk.begin(lineStr);
// is this line blank?
if (tk.eof()) continue;
/* parse line as
<final state>
<initial state> <next state> <transition symbol>
*/
const char *errMsg = "Problem with format of source line";
int s0 = -1;
int s1 = -1;
try {
tk.readWord(stateA);
s0 = Utils::parseInt(stateA);
// if no more data on this line, it's a <final state>
if (tk.eof()) {
makeStateFinal(s0);
} else {
// read the next state & transition symbol
tk.readWord(stateB);
s1 = Utils::parseInt(stateB);
tk.readWord(symbol);
// check validity of input...
if (symbol.length() != 1)
throw Exception(errMsg);
// add a transition symbol between these states; if a symbol
// already exists, it will throw an exception (it's supposed
// to be a DFA, not an NFA)
// if the states don't exist, they will be created.
addTransition(s0,s1,symbol.charAt(0));
}
} catch (StringReaderException &e) {
// REF(e);
throw Exception(errMsg);
} catch (NumberFormatException &e) {
// REF(e);
throw Exception(errMsg);
}
// replace startState if necessary
if (startSt < 0 || s0 == 0)
startSt = s0;
}
if (startSt >= 0)
setStartState(startSt);
}
