bool shutdown_vicon()
{
ROS_INFO_STREAM("Disconnecting from Vicon DataStream SDK");
MyClient.Disconnect();
ROS_ASSERT(!MyClient.IsConnected().Connected);
ROS_INFO_STREAM("... disconnected.");
return true;
}
bool init_vicon()
{
ROS_INFO_STREAM("Connecting to Vicon DataStream SDK at " << HostName << " ...");
while (!MyClient.IsConnected().Connected)
{
MyClient.Connect(HostName);
ROS_INFO(".");
sleep(1);
ros::spinOnce();
if (!ros::ok())
return false;
}
ROS_ASSERT(MyClient.IsConnected().Connected);
ROS_INFO_STREAM("... connected!");
ROS_INFO_STREAM("Setting Stream Mode to " << StreamMode);
{
Output_SetStreamMode result;
if (StreamMode == "ServerPush")
{
result = MyClient.SetStreamMode(StreamMode::ServerPush);
}
else if (StreamMode == "ClientPull")
{
result = MyClient.SetStreamMode(StreamMode::ClientPull);
}
else if (StreamMode == "ClientPullPreFetch")
{
result = MyClient.SetStreamMode(StreamMode::ClientPullPreFetch);
}
else
{
ROS_FATAL("Unknown stream mode -- options are ServerPush, ClientPull, ClientPullPreFetch");
ros::shutdown();
}
if (result.Result != Result::Success)
{
ROS_FATAL("Set stream mode call failed -- shutting down");
ros::shutdown();
}
}
{
Output_SetAxisMapping result;
result = MyClient.SetAxisMapping(Direction::Forward, Direction::Left, Direction::Up); // 'Z-up'
if (result.Result != Result::Success)
{
ROS_FATAL("Set axis mapping call failed -- shutting down");
ros::shutdown();
}
Output_GetAxisMapping _Output_GetAxisMapping = MyClient.GetAxisMapping();
ROS_INFO_STREAM(
"Axis Mapping: X-" << Adapt(_Output_GetAxisMapping.XAxis) << " Y-"
<< Adapt(_Output_GetAxisMapping.YAxis) << " Z-" << Adapt(_Output_GetAxisMapping.ZAxis));
Output_GetVersion _Output_GetVersion = MyClient.GetVersion();
ROS_INFO_STREAM(
"Version: " << _Output_GetVersion.Major << "." << _Output_GetVersion.Minor << "."
<< _Output_GetVersion.Point);
}
return true;
}
