/// set the positions and speeds of the servos
void ActuatorsODE::setPositionsAndSpeeds(const std::map<MotorID, Degree> &positions, const std::map<MotorID, RPM> &speeds)
{
CriticalSectionLock csl(m_cs);
mTargetAngles = positions;
mTargetSpeeds = speeds;
}
/// get all (current) motor speeds
std::map<MotorID, RPM> ActuatorsODE::getSpeeds() const
{
CriticalSectionLock csl(m_cs);
std::map<MotorID, RPM> retMap;
for (std::pair<MotorID, MotorData> const& motor : motorData) {
retMap[motor.first] = motor.second.speed;
}
return retMap;
}
/// get all motor positions
std::map<MotorID, Degree> ActuatorsODE::getPositions() const
{
CriticalSectionLock csl(m_cs);
std::map<MotorID, Degree> retMap;
for (std::pair<MotorID, MotorData> const& motor : motorData) {
retMap[motor.first] = motor.second.position;
}
return retMap;
}
ActuatorsODE::ActuatorsODE(PhysicsEnvironment *environment, KinematicTree *tree)
: m_physicsEnvironment(environment)
, m_tree(tree)
{
m_physicsEnvironment->addStepCallback(this);
CriticalSectionLock csl(m_cs);
std::map<MotorID, ODEMotor*>& physicsMotors = m_physicsEnvironment->getMotors();
for (std::pair<MotorID, ODEMotor*> const& motor: physicsMotors)
{
motorData[motor.first].position = motor.second->getCurAngle();
motorData[motor.first].speed = motor.second->getCurSpeed();
}
}
void ActuatorsODE::simulatorCallback(Second timeDelta)
{
CriticalSectionLock csl(m_cs);
std::map<MotorID, ODEMotor*>& physicsMotors = m_physicsEnvironment->getMotors();
for (std::pair<MotorID, ODEMotor*> const& motor: physicsMotors)
{
motorData[motor.first].position = motor.second->getCurAngle();
motorData[motor.first].speed = motor.second->getCurSpeed();
}
for (std::pair<MotorID, Degree> motor : mTargetAngles)
{
std::map<MotorID, ODEMotor*>::iterator iter = physicsMotors.find(motor.first);
if (iter != physicsMotors.end())
{
iter->second->setDesiredAngle(motor.second);
}
}
for (std::pair<MotorID, RPM> motor : mTargetSpeeds)
{
std::map<MotorID, ODEMotor*>::iterator iter = physicsMotors.find(motor.first);
if (iter != physicsMotors.end())
{
iter->second->setDesiredSpeed(motor.second);
}
}
for (std::pair<MotorID, bool> motor : mTorqueEnabled)
{
std::map<MotorID, ODEMotor*>::iterator iter = physicsMotors.find(motor.first);
if (iter != physicsMotors.end())
{
iter->second->enableMotor(motor.second);
}
}
for (std::pair<MotorID, ODEMotor*> const& motor: physicsMotors)
{
motor.second->simulatorCallback(timeDelta);
}
}
void TEST_circular_sorted_list()
{
std::cout << "TEST circular sorted list.\n";
CSList csl(2);
csl.printList();
Node *m = csl.getMinNode();
csl.insert(m, 4);
csl.printList();
Node *p;
// insert another node
p = csl.getRandomNode();
csl.insert(p, 1);
csl.printList();
// insert another node
p = csl.getRandomNode();
csl.insert(p, 2);
csl.printList();
}
/// enable/disable the torque
void ActuatorsODE::setTorqueEnabled(const std::map<MotorID, bool> &motors)
{
CriticalSectionLock csl(m_cs);
mTorqueEnabled = motors;
}
/// get all motor data (positions/speed)
std::map<MotorID, MotorData> ActuatorsODE::getMotorData() const
{
CriticalSectionLock csl(m_cs);
return std::map<MotorID, MotorData>(motorData);
}
void Robot::Process_Register(rapidjson::Document& jsRequest, rapidjson::Document& jsResponse, string& sResponse)
{
rapidjson::Document::AllocatorType& allocator = jsResponse.GetAllocator();
int nErrcode = 0;
string sErrmsg = "ok";
if (!IsRegistered())
{
jsResponse.AddMember("question_bank_version", "1", allocator);
string sRobotid = "";
if (jsRequest.HasMember("body") && jsRequest["body"].IsObject())
{
rapidjson::Value& body = jsRequest["body"];
if (body.HasMember("robot_id") && body["robot_id"].IsString())
{
sRobotid = body["robot_id"].GetString();
}
else if (jsRequest.HasMember("mac") && jsRequest["mac"].IsObject())
{
sRobotid = body["mac"].GetString();
}
if (!sRobotid.empty())
{
// 清除robot_id对应的旧链接
SAContainerSingleLock csl(&RobotService::instance().m_robotManager.m_mapRobot, true);
SPRobotPtr spRobot = RobotService::instance().m_robotManager.FindRobot(sRobotid);
bool bSuccess = true;
if (spRobot)
{
if (spRobot->isCreateisElapsed(10000))
{ // 旧链接创建超过10s,可踢掉
log.error( format("break session '%s'",spRobot->GetRobotID()));
spRobot->Close();
spRobot->SetValid(false);
}
else
{
log.warning(format("connect frequency too high,reserve old connection'%s'!", spRobot->GetRobotID()));
SATCPConnectionPtr spSocket = m_spSocket;
spSocket->Close();
bSuccess = false;
nErrcode = 6;
sErrmsg = "connect frequency too high,reserve old connection!";
}
}
if (bSuccess)
{
m_bIsRegistered = true;
m_sID = sRobotid;
RobotService::instance().m_robotManager.AddRobot(GetRobotPtr());
//RobotService::instance().m_mapUnkownList.RemoveKey(m_sClient);
}
csl.Unlock();
}
else
{
nErrcode = 8;
sErrmsg = "invalid msg data,miss robot_id param!";
log.warning("invalid msg data,miss robot_id param!");
}
}
else
{
nErrcode = 7;
sErrmsg = "invalid msg data,miss necessary param!";
log.warning("invalid msg data,miss necessary param!");
}
}
else
{
nErrcode = 5;
sErrmsg = "Process register failed,already registered!";
log.error("Process register failed,already registered!");
}
jsResponse.AddMember("errcode", 0, allocator);
jsResponse.AddMember("errmsg", "ok", allocator);
sResponse = JsonDocToString(jsResponse);
}
