///@brief Initialize this controller.
void AvatarControllerByKinectV2::onInit(InitEvent &evt)
{
this->readIniFileAndInitialize();
SimObj *myself = getObj(myname());
this->kinectV2DeviceManager.initPositionAndRotation(myself);
}
///@brief Initialize this controller.
void WheelControllerByMyo::onInit(InitEvent &evt)
{
this->readIniFileAndInitialize();
this->myoDeviceManager = MyoDeviceManager(this->serviceName, this->deviceType, this->deviceUniqueID);
//For initialize.
getObj(myname());
}
double RobotController::onAction(ActionEvent &evt)
{
SimObj *obj = getObj(myname());
if (obj) {
obj->setAccel(0.0, 100.0, 0.0);
}
return 5.0;
}
///@brief Initialize this controller.
void AvatarControllerByOculusCV1::onInit(InitEvent &evt)
{
readIniFileAndInitialize();
this->oculusCV1DeviceManager = OculusCV1DeviceManager(this->serviceName, this->deviceType, this->deviceUniqueID);
//For initialize.
getObj(myname());
}
///@brief Initialize this controller.
void AvatarControllerByPerceptionNeuron::onInit(InitEvent &evt)
{
this->readIniFileAndInitialize();
this->perceptionNeuronDeviceManager = PerceptionNeuronDeviceManager(this->serviceName, this->deviceType, this->deviceUniqueID);
SimObj *myself = getObj(myname());
this->perceptionNeuronDeviceManager.getInitialPositionAndRotation(myself);
}
void RobotController::onInit(InitEvent &evt)
{
joint_veloc = 0.8;
my = getRobotObj(myname());
// 車輪間距離
m_distance = 10.0;
// 車輪半径
m_radius = 10.0;
// 車輪の半径と車輪間距離設定
my->setWheel(m_radius, m_distance);
m_grasp_left = false;
Change_Robot_speed = 5; // to change the robot's velocity
Robot_speed = Change_Robot_speed;
m_state = 0;
// objects position
m_BottleFront = Vector3d(40.0, 30, -40.0);
m_MuccupFront = Vector3d(0.0, 30, -40.0);
m_CanFront = Vector3d(-40.0, 30, -40.0);
// trashBoxs position
m_BurnableFront = Vector3d(-120.0, 30, -60);
m_UnburnableFront = Vector3d(-120.0, 30, 70);
m_RecycleFront = Vector3d(-60.0, 30, -100);
m_relayPoint1 = Vector3d(100, 30, -70);
m_relayPoint2 = Vector3d(0, 30, -70);
m_relayFrontTable = Vector3d(0, 30,-20);
m_relayFrontTable_reset = Vector3d(-80, 30,-50);
m_relayFrontTrash = Vector3d(-80, 30, -80);
cycle = 27;
m_onActionReturn = 0.01;
srand((unsigned)time( NULL ));
// grasp初期化
m_grasp = false;
m_trashes.push_back("petbottle");
m_trashes.push_back("mugcup");
m_trashes.push_back("can");
// ゴミ箱登録
m_trashboxs.push_back("recycle");
m_trashboxs.push_back("burnable");
m_trashboxs.push_back("unburnable");
}
void WheelControllerByMyo::onRecvMsg(RecvMsgEvent &evt)
{
const std::string allMsg = evt.getMsg();
// std::cout << allMsg << std::endl;
std::map<std::string, std::vector<std::string> > sensorDataMap = MyoSensorData::decodeSensorData(allMsg);
if (sensorDataMap.find(MSG_KEY_DEV_TYPE) == sensorDataMap.end()){ return; }
if(sensorDataMap[MSG_KEY_DEV_TYPE][0] !=this->myoDeviceManager.deviceType ){ return; }
if(sensorDataMap[MSG_KEY_DEV_UNIQUE_ID][0]!=this->myoDeviceManager.deviceUniqueID){ return; }
MyoSensorData sensorData;
sensorData.setSensorData(sensorDataMap);
// std::cout << "roll=" << sensorData.roll << ",pitch=" << sensorData.pitch << ",yaw=" << sensorData.yaw << ",pose=" << sensorData.poseStr << std::endl;
// std::cout << "emg1=" << sensorData.emgData[0] << ",emg2=" << sensorData.emgData[1] << ", emg3=" << sensorData.emgData[2] << std::endl;
SimObj *obj = getObj(myname());
float vel = 0.0;
for(int i=0; i<MyoSensorData::EMG_SENSOR_NUM; i++)
{
vel += std::abs(sensorData.emgData[i]);
}
double rvel=0.0, lvel=0.0;
// Turn left.
if(sensorData.poseStr==MyoSensorData::mapPoseType2Str.at(MyoSensorData::WaveIn))
{
rvel = 3.0*vel;
lvel = 0.05*vel;
}
// Turn right.
if(sensorData.poseStr==MyoSensorData::mapPoseType2Str.at(MyoSensorData::WaveOut))
{
rvel = 0.05*vel;
lvel = 3.0*vel;
}
// Brake.
if(sensorData.poseStr==MyoSensorData::mapPoseType2Str.at(MyoSensorData::Fist))
{
rvel = -3.0*vel;
lvel = -3.0*vel;
}
// std::cout << "rvel=" << rvel << ",lvel=" << lvel << std::endl;
obj->setJointVelocity("JOINT_RWHEEL", rvel, 500.0);
obj->setJointVelocity("JOINT_LWHEEL", lvel, 500.0);
}
TSMWarning *BuckeyeSMPlugin::convertISDToSensorModelState(
const tsm_ISD& image_support_data,
const std::string& sensor_model_name,
std::string& sensor_model_state) const throw (TSMError)
{
std::string myname("BuckeyeSMPlugin::convertISDToSensorModelState");
TSMError tsmError(TSMError::UNKNOWN_ERROR, "Unable to convert ISD to sensor model state.", myname);
throw tsmError;
return NULL;
}
//定期的に呼び出される関数です。
double CameraController::onAction(ActionEvent &evt)
{
// ロボット位置取得
r_my->getPosition(r_pos);
// カメラ番号取得
m_my = getObj(myname());
m_name = m_my->name();
// 定点カメラをロボットの方向に回転
rotateTowardRobot(r_pos);
return 0.05;
}
void UserController::moveRightArm(int task)
{
SimObj *my = getObj(myname());
my->setJointAngle("RARM_JOINT0", tab_joint_right[task][0]);
my->setJointAngle("RARM_JOINT1", tab_joint_right[task][1]);
my->setJointAngle("RARM_JOINT2", tab_joint_right[task][2]);
my->setJointAngle("RARM_JOINT3", tab_joint_right[task][3]);
my->setJointAngle("RARM_JOINT4", tab_joint_right[task][4]);
my->setJointAngle("RARM_JOINT5", tab_joint_right[task][5]);
my->setJointAngle("RARM_JOINT6", tab_joint_right[task][6]);
my->setJointAngle("RARM_JOINT7", tab_joint_right[task][7]);
}
str
sfshostname ()
{
str name = safegetenv ("SFS_HOSTNAME");
if (!name)
name = myname ();
if (!name)
fatal ("could not figure out host's fully-qualified domain name\n");
mstr m (name.len ());
for (u_int i = 0; i < name.len (); i++)
m[i] = tolower (name[i]);
return m;
}
/*
* Find unreferenced link names.
*/
void
findunreflinks(void)
{
struct entry *ep, *np;
ino_t i;
vprintf(stdout, "Find unreferenced names.\n");
for (i = ROOTINO; i < maxino; i++) {
ep = lookupino(i);
if (ep == NULL || ep->e_type == LEAF || TSTINO(i, dumpmap) == 0)
continue;
for (np = ep->e_entries; np != NULL; np = np->e_sibling) {
if (np->e_flags == 0) {
dprintf(stdout,
"%s: remove unreferenced name\n",
myname(np));
removeleaf(np);
freeentry(np);
}
}
}
/*
* Any leaves remaining in removed directories is unreferenced.
*/
for (ep = removelist; ep != NULL; ep = ep->e_next) {
for (np = ep->e_entries; np != NULL; np = np->e_sibling) {
if (np->e_type == LEAF) {
if (np->e_flags != 0)
badentry(np, "unreferenced with flags");
dprintf(stdout,
"%s: remove unreferenced name\n",
myname(np));
removeleaf(np);
freeentry(np);
}
}
}
}
void MyController::onInit(InitEvent &evt) {
m_my = getObj(myname());
// この範囲で判定
checkSize_x = 40.0;
checkSize_z = 80.0;
// 自分の位置取得
m_my->getPosition(myPos);
sentMsg = false;
flag1 = false;
flag2 = false;
}
void CameraController::onInit(InitEvent &evt)
{
// カメラ初期方向 1:45 2:135 3:315 4:225
// ロボット初期位置取得
r_my = getRobotObj("robot_004");
r_my->getPosition(r_pos);
// カメラ番号取得
m_my = getObj(myname());
m_name = m_my->name();
// 定点カメラをロボットの方向に回転
rotateTowardRobot(r_pos);
}
/*
* Create a new node (directory).
*/
void
newnode(struct entry *np)
{
char *cp;
if (np->e_type != NODE)
badentry(np, "newnode: not a node");
cp = myname(np);
if (!Nflag && mkdir(cp, 0777) < 0) {
np->e_flags |= EXISTED;
fprintf(stderr, "warning: %s: %s\n", cp, strerror(errno));
return;
}
vprintf(stdout, "Make node %s\n", cp);
}
void MyController::onInit(InitEvent &evt) {
m_my = getObj(myname());
getAllEntities(m_entities);
m_ref = NULL;
retValue = 0.1;
roboName = "robot_000";
colState = false;
// ゴミ箱
tboxSize_x = 120.0;
tboxSize_z = 90.5;
tboxMin_y = 30.0;
tboxMax_y = 1000.0;
}
void MyController::onInit(InitEvent &evt) {
m_my = getObj(myname());
getAllEntities(m_entities);
m_ref = NULL;
retValue = 0.5;
roboName = "robot_000";
colState = false;
// ワゴン
tboxSize_x = 60.6;
tboxSize_z = 64.6;
tboxMin_y = 50.8;
tboxMax_y = 1000.0;
}
/*
* Create a new node (directory).
*/
void
newnode(struct entry *np)
{
char *cp;
if (np->e_type != NODE)
badentry(np, "newnode: not a node");
cp = myname(np);
if (!Nflag && mkdir(cp, 0777) < 0) {
np->e_flags |= EXISTED;
warn("%s", cp);
return;
}
Vprintf(stdout, "Make node %s\n", cp);
}
