int main( int argc, char* argv[] )
{
// Initialize ROS node
ros::init(argc, argv, "mocap_node", ros::init_options::NoSigintHandler );
ros::NodeHandle n("~");
// Get configuration from ROS parameter server
const char** mocap_model( DEFAULT_MOCAP_MODEL );
if( n.hasParam( MOCAP_MODEL_KEY ) )
{ std::string tmp;
if( n.getParam( MOCAP_MODEL_KEY, tmp ) )
{
if( tmp == "SKELETON_WITH_TOES" )
mocap_model = SKELETON_WITH_TOES;
else if( tmp == "SKELETON_WITHOUT_TOES" )
mocap_model = SKELETON_WITHOUT_TOES;
else if( tmp == "OBJECT" )
mocap_model = OBJECT;
}
}
// Process mocap data until SIGINT
processMocapData( mocap_model );
return 0;
}
int main( int argc, char* argv[] )
{
// Initialize ROS node
ros::init(argc, argv, "mocap_node");
ros::NodeHandle n("~");
// Get configuration from ROS parameter server
const char** mocap_model( DEFAULT_MOCAP_MODEL );
if( n.hasParam( MOCAP_MODEL_KEY ) )
{ std::string tmp;
if( n.getParam( MOCAP_MODEL_KEY, tmp ) )
{
if( tmp == "SKELETON_WITH_TOES" )
mocap_model = SKELETON_WITH_TOES;
else if( tmp == "SKELETON_WITHOUT_TOES" )
mocap_model = SKELETON_WITHOUT_TOES;
else if( tmp == "OBJECT" )
mocap_model = OBJECT;
}
}
RigidBodyMap published_rigid_bodies;
if (n.hasParam(RIGID_BODIES_KEY))
{
XmlRpc::XmlRpcValue body_list;
n.getParam("rigid_bodies", body_list);
if (body_list.getType() == XmlRpc::XmlRpcValue::TypeStruct && body_list.size() > 0)
{
XmlRpc::XmlRpcValue::iterator i;
for (i = body_list.begin(); i != body_list.end(); ++i) {
if (i->second.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
PublishedRigidBody body(i->second);
string id = (string&) (i->first);
RigidBodyItem item(atoi(id.c_str()), body);
std::pair<RigidBodyMap::iterator, bool> result = published_rigid_bodies.insert(item);
if (!result.second)
{
ROS_ERROR("Could not insert configuration for rigid body ID %s", id.c_str());
}
}
}
}
}
//Create other marker publisher
ros::Publisher otherMarkerPublisher = n.advertise<mocap_optitrack::MarkerMsg>("OtherMarkers", 1000);
// Process mocap data until SIGINT
processMocapData(mocap_model, published_rigid_bodies, otherMarkerPublisher);
return 0;
}
int main( int argc, char* argv[] )
{
// Initialize ROS node
ros::init(argc, argv, "mocap_node");
ros::NodeHandle n("~");
// Get configuration from ROS parameter server
const char** mocap_model( DEFAULT_MOCAP_MODEL );
if( n.hasParam( MOCAP_MODEL_KEY ) )
{ std::string tmp;
if( n.getParam( MOCAP_MODEL_KEY, tmp ) )
{
if( tmp == "SKELETON_WITH_TOES" )
mocap_model = SKELETON_WITH_TOES;
else if( tmp == "SKELETON_WITHOUT_TOES" )
mocap_model = SKELETON_WITHOUT_TOES;
else if( tmp == "OBJECT" )
mocap_model = OBJECT;
}
}
// Get configuration from ROS parameter server
std::string multicast_ip( MULTICAST_IP_DEFAULT );
if( n.hasParam( MULTICAST_IP_KEY ) )
{
n.getParam( MULTICAST_IP_KEY, multicast_ip );
}
else {
ROS_WARN_STREAM("Could not get multicast address, using default: " << multicast_ip);
}
RigidBodyMap published_rigid_bodies;
if (n.hasParam(RIGID_BODIES_KEY))
{
XmlRpc::XmlRpcValue body_list;
n.getParam("rigid_bodies", body_list);
if (body_list.getType() == XmlRpc::XmlRpcValue::TypeStruct && body_list.size() > 0)
{
XmlRpc::XmlRpcValue::iterator i;
for (i = body_list.begin(); i != body_list.end(); ++i) {
if (i->second.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
PublishedRigidBody body(i->second);
string id = (string&) (i->first);
RigidBodyItem item(atoi(id.c_str()), body);
std::pair<RigidBodyMap::iterator, bool> result = published_rigid_bodies.insert(item);
if (!result.second)
{
ROS_ERROR("Could not insert configuration for rigid body ID %s", id.c_str());
}
}
}
}
}
// Process mocap data until SIGINT
processMocapData(mocap_model, published_rigid_bodies, multicast_ip);
return 0;
}
//==============================================================================
void MyWindow::timeStepping()
{
processMocapData();
bool isFirst6DofsValid = false;// fromMarkersTo6Dofs();
Eigen::VectorXd motor_qhat = mController->motion()->targetPose(mTime);
Eigen::VectorXd fullMotorAngle = Eigen::VectorXd::Zero(NUM_MOTORS + 2);
if (!mSimulating)
{
motor_qhat = mController->motion()->getInitialPose();
}
// Wait for an event
Eigen::VectorXd motorAngle;
motorAngle = Eigen::VectorXd::Zero(NUM_MOTORS);
//if (mTime < 3)
// motor_qhat = mController->useAnkelStrategy(motor_qhat, mTime);
//Eigen::VectorXd mocapPose = mController->mocap()->GetPose(mTime - 1.0);
//Eigen::VectorXd motor_qhat = mController->motormap()->toMotorMapVectorSameDim(mocapPose);
Eigen::VectorXd motor_qhat_noGriper = Eigen::VectorXd::Zero(NUM_MOTORS);
motor_qhat_noGriper.head(6) = motor_qhat.head(6);
motor_qhat_noGriper.tail(10) = motor_qhat.tail(10);
unsigned char commands[256];
for (int i = 0; i < NUM_MOTORS; ++i)
{
int command = static_cast<int>(motor_qhat_noGriper[i]);
unsigned char highByte, lowByte;
SeparateWord(command, &highByte, &lowByte);
commands[2 * i] = lowByte;
commands[2 * i + 1] = highByte;
//commands[3 * i + 2] = ' ';
}
commands[2 * 16] = '\t';
commands[2 * 16 + 1] = '\n';
mSerial->Write(commands, 2 * 16 + 2);
DWORD dwBytesRead = 0;
char szBuffer[101];
bool bUpdated = false;
do
{
// Read data from the COM-port
LONG lLastError = mSerial->Read(szBuffer, sizeof(szBuffer) - 1, &dwBytesRead);
if (lLastError != ERROR_SUCCESS)
LOG(FATAL) << mSerial->GetLastError() << " Unable to read from COM-port.";
if (dwBytesRead > 0)
{
mTmpBuffer.insert(mTmpBuffer.size(), szBuffer, dwBytesRead);
if (mTmpBuffer.size() >= NUM_BYTES_PER_MOTOR * NUM_MOTORS + 1)
{
bUpdated = ProcessBuffer(mTmpBuffer, motorAngle);
if (bUpdated)
{
fullMotorAngle.head(6) = motorAngle.head(6);
fullMotorAngle.tail(10) = motorAngle.tail(10);
mController->setMotorMapPose(fullMotorAngle);
}
//for (int i = 0; i < 3; ++i)
//{
// std::cout << motorAngle[i] << " "; //<< motor_qhat_noGriper[14];
//}
//std::cout << std::endl;
}
}
else
{
std::cout << "Noting received." << endl;
}
} while (dwBytesRead == sizeof(szBuffer) - 1);
if (isFirst6DofsValid)
mController->setFreeDofs(mFirst6DofsFromMocap);
if (mSimulating)
{
Eigen::VectorXd poseToRecord = mController->robot()->getPositions();
MocapFrame frame;
frame.mTime = mTime;
frame.mMarkerPos = mMarkerPos;
frame.mMarkerOccluded = mMarkerOccluded;
frame.mMotorAngle = poseToRecord;
mRecordedFrames.push_back(frame);
}
//mController->keepFeetLevel();
if (mTimer.isStarted())
{
double elaspedTime = mTimer.getElapsedTime();
//LOG(INFO) << elaspedTime;
LOG(INFO) << mTime << " " << motor_qhat[10] << " " << fullMotorAngle[10] << endl;
mTime += elaspedTime;// mController->robot()->getTimeStep();
}
mTimer.start();
}
