int main(int argc, char** argv)
{
ros::init(argc, argv, "pr2_right_arm_mocap_servoing_controller");
ROS_INFO("Starting pr2_right_arm_mocap_servoing_controller (w/o mocap)...");
ros::NodeHandle nh;
ros::NodeHandle nhp("~");
std::string target_pose_topic;
std::string arm_config_topic;
std::string arm_command_action;
std::string abort_service;
//double execution_timestep = 0.1;
double kp = DEFAULT_KP;
double ki = DEFAULT_KI;
double kd = DEFAULT_KD;
nhp.param(std::string("target_pose_topic"), target_pose_topic, std::string("/r_arm_pose_controller/target"));
nhp.param(std::string("arm_config_topic"), arm_config_topic, std::string("/r_arm_controller/state"));
nhp.param(std::string("arm_command_action"), arm_command_action, std::string("/r_arm_controller/joint_trajectory_action"));
nhp.param(std::string("abort_service"), abort_service, std::string("/r_arm_pose_controller/abort"));
//nhp.param(std::string("execution_timestep"), execution_timestep, 0.1);
nhp.param(std::string("kp"), kp, DEFAULT_KP);
nhp.param(std::string("ki"), ki, DEFAULT_KI);
nhp.param(std::string("kd"), kd, DEFAULT_KD);
ROS_INFO("Running in INTERNAL_POSE mode");
pr2_mocap_servoing::MocapServoingController controller(nh, std::string("right_arm"), target_pose_topic, arm_config_topic, arm_command_action, abort_service, kp, ki, kd);
ROS_INFO("...startup complete");
controller.Loop();
return 0;
}
EasyLight::EasyLight()
{
ros::NodeHandle nhp("~");
on_sub_ = nh.subscribe < std_msgs::Bool > ("/pushed", 2, &EasyLight::onCallback, this);
on_pub = nh.advertise < std_msgs::Empty > ("/MILIGHT/light3ON", 2);
off_pub = nh.advertise < std_msgs::Empty > ("/MILIGHT/light3OFF", 2);
lastOff = ros::Time::now();
enableOff.data = false;
}
Pathwrapper::Pathwrapper()
{
sem = 1;
ros::NodeHandle nhp("~");
nhp.getParam("map_name", map_name);
nhp.getParam("base_name", base_name);
nhp.param<int>("nb_step_skip", NB_STEP_SKIP, 10);
nhp.param<double>("max_dist_skip", MAX_DIST_SKIP, 0.20);
// Goal suscriber
goal_sub_ = nh.subscribe < geometry_msgs::PoseStamped > ("/move_base_test/goal", 20, &Pathwrapper::goalCallback, this);
// Path suscriber
//path_sub_ = nh.subscribe < nav_msgs::Path > ("/move_base/TrajectoryPlannerROS/global_plan", 20, &MaximusPath::pathCallback, this);
path_sub_ = nh.subscribe < nav_msgs::Path > ("/ROBOT/plan", 20, &Pathwrapper::pathCallback, this);
pose2D_pub = nh.advertise < geometry_msgs::Pose2D > ("/ROBOT_goal", 1);
poseArray_pub = nh.advertise < geometry_msgs::PoseArray > ("/poses", 50);
//arduGoal_pub = nh.advertise < common_smart_nav::ArduGoal > ("/ROBOT/ardugoal", 1);
arduGoal_pub = nh.advertise < geometry_msgs::Pose2D > ("/ROBOT/ardugoal", 1);
pathdone_pub = nh.advertise < std_msgs::Empty > ("/ROBOT/path_done", 50);
pause_planner_pub = nh.advertise < std_msgs::Empty > ("/pause_planner", 5);
pause_sub_ = nh.subscribe < std_msgs::Empty > ("/ROBOT/pause_nav", 20, &Pathwrapper::pauseCallback, this);
resume_sub_ = nh.subscribe < std_msgs::Empty > ("/ROBOT/resume_nav", 20, &Pathwrapper::resumeCallback, this);
pause = 0;
my_path.poses = std::vector < geometry_msgs::PoseStamped > ();
if (my_path.poses.std::vector < geometry_msgs::PoseStamped >::size() >
(my_path.poses.std::vector < geometry_msgs::PoseStamped >::max_size() - 2)) {
my_path.poses.std::vector < geometry_msgs::PoseStamped >::pop_back();
}
final_pose.x = 0.0;
final_pose.y = 0.14;
final_pose.theta = 0.0;
final_ardugoal.x = 0.0;
final_ardugoal.y = 0.0;
final_ardugoal.theta = 0.0;
//final_ardugoal.last = 0;
cpt_send = 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "waypoint_source");
ros::NodeHandle nh;
ros::NodeHandle nhp("~");
ROS_INFO_STREAM("Has param: " << nhp.hasParam("file"));
std::string path;
nhp.getParam("file", path);
ROS_INFO_STREAM("Loading waypoints from " << path);
waypoint_pub = nh.advertise<geometry_msgs::PointStamped>("/waypoint", 1);
ros::Subscriber odom_sub = nh.subscribe("/odometry/filtered", 1, positionCallback);
ros::Subscriber origin_sub = nh.subscribe("/gps/filtered", 1, originCallback);
loadWaypointsFile(path, waypoints);
if (!waypoints.empty())
{
ROS_INFO_STREAM(waypoints.size() << " waypoints found.");
current_waypoint = waypoints.front();
ros::Rate rate(1); // 1 Hz
while (ros::ok())
{
{
std::lock_guard<std::mutex> lock(current_mutex);
auto waypoint_for_pub = current_waypoint;
waypoint_for_pub.header.stamp = ros::Time::now();
waypoint_for_pub.header.seq++;
waypoint_for_pub.header.frame_id = "odom";
waypoint_for_pub.point.x -= map_origin.x;
waypoint_for_pub.point.y -= map_origin.y;
waypoint_pub.publish(waypoint_for_pub);
}
ros::spinOnce();
rate.sleep();
}
}
else
{
ROS_ERROR("No valid waypoint entries found.");
}
return 0;
}
Dash::Dash()
{
ros::NodeHandle nhp("~");
//nhp.getParam("base_name", base_name);
nhp.param<std::string>("address", address, "192.168.0.1");
nhp.param<std::string>("port", port, "3030");
nhp.param<std::string>("type", type, "value");
nhp.param<std::string>("dash_topic", dash_topic, "synergie");
float_sub_ = nh.subscribe < std_msgs::Float32 > ("/DASHBOARD/test", 1, &Dash::floatCallback, this);
value_save = 0.0;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "navigation_controller");
ros::NodeHandle nh;
ros::NodeHandle nhp("~");
state = WAITING_FOR_START;
planSpeed = 0.0;
planSteering = 0.0;
finishLineCrosses = 0;
nhp.getParam("req_finish_line_crosses", REQ_FINISH_LINE_CROSSES);
nhp.getParam("startSignal", startSignal);
nhp.getParam("resetSignal", resetSignal);
ROS_INFO("required finish line crosses = %d", REQ_FINISH_LINE_CROSSES);
auto planSpeedSub = nh.subscribe("plan/speed", 1, planSpeedCB);
auto planSteerSub = nh.subscribe("plan/steering", 1, planSteerCB);
auto startLightSub = nh.subscribe(startSignal, 1, startLightCB);
auto finishLineSub = nh.subscribe("/camera/finish_line_crosses", 1, finishLineCB);
auto resetSub = nh.subscribe(resetSignal, 1, resetCB);
speedPub = nh.advertise<rr_platform::speed>("/speed", 1);
steerPub = nh.advertise<rr_platform::steering>("/steering", 1);
ros::Rate rate(30.0);
while(ros::ok()) {
ros::spinOnce();
updateState();
//ROS_INFO("Nav Mux = %d, crosses = %d", state, finishLineCrosses);
rr_platform::speed speedMsg;
speedMsg.speed = speed;
speedMsg.header.stamp = ros::Time::now();
speedPub.publish(speedMsg);
rr_platform::steering steerMsg;
steerMsg.angle = steering;
steerMsg.header.stamp = ros::Time::now();
steerPub.publish(steerMsg);
ROS_INFO("Current state: %d", state);
rate.sleep();
}
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "lidar_aggregator");
ros::NodeHandle nh;
ros::NodeHandle nhp("~");
tf::TransformListener listener(nh, ros::Duration(20.0));
g_transformer = &listener;
ROS_INFO("Starting LIDAR aggregator...");
nhp.param(std::string("fixed_frame"), g_fixed_frame, std::string("Body_TSY"));
g_cloud_publisher = nh.advertise<sensor_msgs::PointCloud2>("pointcloud", 1, true);
ros::ServiceServer server = nh.advertiseService("aggregate_lidar", LaserAggregationServiceCB);
ROS_INFO("LIDAR aggregator loaded");
while (ros::ok())
{
ros::spinOnce();
}
ROS_INFO("Shutting down LIDAR aggregator");
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "sync_node");
ros::NodeHandle nh;
ros::NodeHandle nhp("~");
avt_vimba_camera::Sync sync(nh,nhp);
boost::thread syncThread(&avt_vimba_camera::Sync::run, &sync);
// ROS spin
ros::Rate r(10);
while (ros::ok())
r.sleep();
ros::shutdown();
return 0;
}
Planner():
tfListener(tfBuffer)
{
ros::NodeHandle nhp("~");
goal_pose_pub = nhp.advertise<geometry_msgs::PoseStamped>("/goal_pose", 1);
pose_sub = nhp.subscribe<geometry_msgs::PoseStamped>("/pose", 1, &poseCb, this);
pose_sub = nhp.subscribe<geometry_msgs::PoseStamped>("/quadtree", 1, &quadtreeCb, this);
getNeighborsClient = nhp.serviceClient<teamShield::GetNeighbors>("get_neighbors");
map_width = nhp.param("map_width" , 100);
map_height = nhp.param("map_height" , 100);
//Sensor configuration settings - Support sonar or lidar inputs ************
//top_sonar_present = nhp.param("top_sonar_present", true);
iteration = 0;
}
bebop_control()
{
// Parameters
ros::NodeHandle nhp("~");
nhp.param<bool>("lazy", lazy, false);
nhp.param<std::string>("target", target, "ugv0");
nhp.param<double>("radius", radius, 1.0);
nhp.param<double>("kp", kp, 0.5);
nhp.param<double>("kw", kw, 1.0);
nhp.param<double>("kpd", kpd, 0.5);
// Publishers
velCmdPub = nh.advertise<geometry_msgs::Twist>("/bebop/cmd_vel",1);
takeoffPub = nh.advertise<std_msgs::Empty>("bebop/takeoff",1);
landPub = nh.advertise<std_msgs::Empty>("bebop/land",1);
resetPub = nh.advertise<std_msgs::Empty>("bebop/reset",1);
// Initialize states
autonomy = false;
mocapOn = false;
bebopVelOn = false;
targetVelOn = false;
// Subscribers
joySub = nh.subscribe("joy",1,&bebop_control::joyCB,this);
poseSub = nh.subscribe("bebop/pose",1,&bebop_control::poseCB,this);
bebopVelSub = nh.subscribe("bebop/vel",1,&bebop_control::bebopVelCB,this);
target1VelSub = nh.subscribe("ugv0/vel",1,&bebop_control::targetVelCB,this);
target2VelSub = nh.subscribe("ugv1/vel",1,&bebop_control::targetVelCB,this);
// Warning message
while (!(ros::isShuttingDown()) and (!mocapOn or !bebopVelOn or !targetVelOn))
{
if (!mocapOn) { std::cout << "BEBOP POSE NOT PUBLISHED! THE WALL IS DOWN!\nIs the mocap on?" << std::endl; }
if (!bebopVelOn) { std::cout << "BEBOP VELOCITY NOT PUBLISHED! => Bad tracking performance\nIs velocity filter running?" << std::endl; }
if (!targetVelOn) { std::cout << "TARGET VELOCITY NOT PUBLISHED! => Bad tracking performance\nIs velocity filter running?" << std::endl; }
ros::spinOnce();
ros::Duration(0.1).sleep();
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "iarrc_motor_relay_node");
ros::NodeHandle nh;
ros::NodeHandle nhp("~");
// Subscribers
std::string speed_topic_name;
nhp.param(std::string("speed_topic"), speed_topic_name, std::string("/speed"));
ros::Subscriber speed_sub = nh.subscribe(speed_topic_name, 1, SpeedCallback);
std::string steering_topic_name;
nhp.param(std::string("steering_topic"), steering_topic_name, std::string("/steering"));
ros::Subscriber steering_sub = nh.subscribe(steering_topic_name, 1, SteeringCallback);
// Publishers
ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("/imu/data_raw", 1);
ros::Publisher mag_pub = nh.advertise<sensor_msgs::MagneticField>("/imu/mag", 1);
ros::Publisher temp_pub = nh.advertise<sensor_msgs::Temperature>("/imu/temperature", 1);
ROS_INFO_STREAM("Listening for speed on " << speed_topic_name);
ROS_INFO_STREAM("Listening for steer on " << steering_topic_name);
// Serial port setup
std::string serial_port_name;
nhp.param(std::string("serial_port"), serial_port_name, std::string("/dev/ttyACM0"));
boost::asio::io_service io_service;
boost::asio::serial_port serial(io_service, serial_port_name);
serial.set_option(boost::asio::serial_port_base::baud_rate(115200));
ROS_INFO("IARRC motor relay node ready.");
float hz = 20;
ros::Rate rate(hz);
int count = 0;
int countLimit = (int) (hz / 10); // Limit motor commands to 10hz regardless of loop rate
int sequence = 0;
while (ros::ok() && serial.is_open()) {
ros::spinOnce();
if (count == countLimit) {
if (desiredSteer != prevAngle || desiredSpeed != prevSpeed) {
ROS_INFO("Sending command: servo=%d, motor=%d", desiredSteer, desiredSpeed);
}
prevAngle = desiredSteer;
prevSpeed = desiredSpeed;
sendCommand(serial);
count = 0;
}
count++;
auto lineIn = readLine(serial);
if (!lineIn.empty()) {
auto tokens = split(lineIn.substr(1), ',');
if (tokens.size() < 14) {
ROS_WARN_STREAM("Received invalid response: " << lineIn.substr(1));
continue;
}
sensor_msgs::Imu imuMsg;
imuMsg.header.seq = sequence++;
imuMsg.header.stamp = ros::Time::now();
imuMsg.header.frame_id = "imu";
imuMsg.linear_acceleration.x = std::stod(tokens[0]);
imuMsg.linear_acceleration.y = std::stod(tokens[1]);
imuMsg.linear_acceleration.z = std::stod(tokens[2]);
imuMsg.angular_velocity.x = std::stod(tokens[3]);
imuMsg.angular_velocity.y = std::stod(tokens[4]);
imuMsg.angular_velocity.z = std::stod(tokens[5]);
imuMsg.orientation.x = std::stod(tokens[6]);
imuMsg.orientation.y = std::stod(tokens[7]);
imuMsg.orientation.z = std::stod(tokens[8]);
imuMsg.orientation.w = std::stod(tokens[9]);
imuMsg.linear_acceleration_covariance = unknown_covariance;
imuMsg.angular_velocity_covariance = unknown_covariance;
imuMsg.orientation_covariance = unknown_covariance;
imu_pub.publish(imuMsg);
sensor_msgs::MagneticField magMsg;
magMsg.header = imuMsg.header;
magMsg.magnetic_field.x = std::stod(tokens[10]);
magMsg.magnetic_field.y = std::stod(tokens[11]);
magMsg.magnetic_field.z = std::stod(tokens[12]);
magMsg.magnetic_field_covariance = unknown_covariance;
mag_pub.publish(magMsg);
sensor_msgs::Temperature tempMsg;
tempMsg.header = imuMsg.header;
tempMsg.temperature = std::stod(tokens[13]);
tempMsg.variance = 0;
temp_pub.publish(tempMsg);
}
rate.sleep();
}
serial.close();
ROS_INFO("Shutting down IARRC motor relay node.");
return 0;
}
int main(int argc, char **argv)
{
// Initialize ROS node
ros::init(argc, argv, "hubo_ros_feedback", ros::init_options::NoSigintHandler);
signal(SIGINT, shutdown);
ros::NodeHandle nh;
ros::NodeHandle nhp("~");
ROS_INFO("Initializing ACH-to-ROS bridge [feedback]");
g_last_clock = 0.0;
// Load joint names and mappings
XmlRpc::XmlRpcValue joint_names;
if (!nhp.getParam("joints", joint_names))
{
ROS_FATAL("No joints given. (namespace: %s)", nhp.getNamespace().c_str());
exit(1);
}
if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_FATAL("Malformed joint specification. (namespace: %s)", nhp.getNamespace().c_str());
exit(1);
}
for (unsigned int i = 0; i < joint_names.size(); ++i)
{
XmlRpc::XmlRpcValue &name_value = joint_names[i];
if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
{
ROS_FATAL("Array of joint names should contain all strings. (namespace: %s)", nhp.getNamespace().c_str());
exit(1);
}
g_joint_names.push_back((std::string)name_value);
}
// Gets the hubo ach index for each joint
for (size_t i = 0; i < g_joint_names.size(); ++i)
{
std::string ns = std::string("mapping/") + g_joint_names[i];
int h;
nhp.param(ns + "/huboachid", h, -1);
g_joint_mapping[g_joint_names[i]] = h;
}
// Set up the ROS publishers
ROS_INFO("Loading publishers...");
g_hubo_state_pub = nh.advertise<hubo_robot_msgs::JointCommandState>(nh.getNamespace() + "/hubo_state", 1);
g_hubo_clock_pub = nh.advertise<rosgraph_msgs::Clock>("clock", 1);
g_body_imu_pub = nh.advertise<sensor_msgs::Imu>(nh.getNamespace() + "/body_imu", 1);
g_left_foot_imu_pub = nh.advertise<sensor_msgs::Imu>(nh.getNamespace() + "/left_foot_tilt", 1);
g_right_foot_imu_pub = nh.advertise<sensor_msgs::Imu>(nh.getNamespace() + "/right_foot_tilt", 1);
g_left_wrist_ft_pub = nh.advertise<geometry_msgs::WrenchStamped>(nh.getNamespace() + "/left_wrist_ft", 1);
g_right_wrist_ft_pub = nh.advertise<geometry_msgs::WrenchStamped>(nh.getNamespace() + "/right_wrist_ft", 1);
g_left_ankle_ft_pub = nh.advertise<geometry_msgs::WrenchStamped>(nh.getNamespace() + "/left_ankle_ft", 1);
g_right_ankle_ft_pub = nh.advertise<geometry_msgs::WrenchStamped>(nh.getNamespace() + "/right_ankle_ft", 1);
ROS_INFO("ROS publishers loaded");
// Make the connection to Hubo-ACH
bool ready = false;
while (!ready)
{
try
{
g_ach_bridge = new ACH_ROS_WRAPPER<hubo_state>(HUBO_CHAN_STATE_NAME);
ready = true;
ROS_INFO("Loaded HUBO-ACH interface");
}
catch (...)
{
ROS_WARN("Could not open ACH interface...retrying in 5 seconds...");
ros::Duration(5.0).sleep();
}
}
ROS_INFO("Starting to stream data from Hubo...");
// Loop
while (ros::ok())
{
try
{
hubo_state robot_state = g_ach_bridge->ReadNextState();
ACHtoHuboState(robot_state);
}
catch (...)
{
ROS_ERROR("Unable to get/process state from hubo-ach");
}
ros::spinOnce();
}
// Satisfy the compiler
return 0;
}
/** \brief Read the node parameters
*/
void readParameters(odom::Tracker::Params &tracker_params)
{
ros::NodeHandle nhp("~");
nhp.param("camera_name", tracker_params.camera_name, string(""));
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName);
ros::NodeHandle nhp("~");
// start a ROS spinning thread
ros::AsyncSpinner spinner(1);
spinner.start();
move_group_interface::MoveGroup group("arm");
group.setPoseReferenceFrame("base_link");
group.setPlannerId("PRMstarkConfigDefault");
group.setPlanningTime(10);
group.setStartStateToCurrentState();
double d_lx, d_ly, d_lz, d_r, d_p, d_y;
nhp.param<double>("lx", d_lx, 0);
nhp.param<double>("ly", d_ly, 0);
nhp.param<double>("lz", d_lz, 0);
nhp.param<double>("r", d_r, 0);
nhp.param<double>("p", d_p, 0);
nhp.param<double>("y", d_y, 0);
geometry_msgs::PoseStamped c = group.getCurrentPose();
std::cout << "current: " << c.pose;
std::vector<double> rpy = group.getCurrentRPY();
std::cout << "r: " << rpy[0];
std::cout << ", p: " << rpy[1];
std::cout << ", y: " << rpy[2];
std::cout << std::endl << std::endl;
geometry_msgs::PoseStamped t = c;
t.pose.position.x += d_lx;
t.pose.position.y += d_ly;
t.pose.position.z += d_lz;
double r = d_r + rpy[0];
double p = d_p + rpy[1];
double y = d_y + rpy[2];
tf::quaternionTFToMsg(tf::createQuaternionFromRPY(r, p, y), t.pose.orientation);
std::cout << "target: " << t.pose;
std::cout << "r: " << r;
std::cout << ", p: " << p;
std::cout << ", y: " << y;
std::cout << std::endl;
group.setPoseTarget(t);
// plan the motion and then move the group to the sampled target
moveit::planning_interface::MoveGroup::Plan plan;
bool found_plan = group.plan(plan);
if (found_plan) {
std::cout << "found a plan!" << std::endl;
group.execute(plan);
}
else
std::cout << "fail." << std::endl;
ros::waitForShutdown();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "raw_marker_bridge");
ROS_INFO("Starting raw marker bridge...");
ros::NodeHandle nh;
ros::NodeHandle nhp("~");
ROS_INFO("Loading parameters...");
std::string tracker_hostname;
std::string tracker_port;
std::string tracker_frame_name;
std::string tracker_name;
std::string tracker_topic;
std::string stream_mode;
nhp.param(std::string("tracker_hostname"), tracker_hostname, std::string("192.168.2.161"));
nhp.param(std::string("tracker_port"), tracker_port, std::string("801"));
nhp.param(std::string("tracker_frame_name"), tracker_frame_name, std::string("mocap_world"));
nhp.param(std::string("tracker_name"), tracker_name, std::string("vicon"));
nhp.param(std::string("tracker_topic"), tracker_topic, std::string("mocap_markers"));
nhp.param(std::string("stream_mode"), stream_mode, std::string("ServerPush"));
// Check the stream mode
if (stream_mode == std::string("ServerPush") || stream_mode == std::string("ClientPullPreFetch") || stream_mode == std::string("ClientPull"))
{
ROS_INFO("Starting in %s mode", stream_mode.c_str());
}
else
{
ROS_FATAL("Invalid stream mode %s, valid options are ServerPush, ClientPullPreFetch, and ClientPull", stream_mode.c_str());
exit(-1);
}
// Assemble the full hostname
tracker_hostname = tracker_hostname + ":" + tracker_port;
ROS_INFO("Connecting to Vicon Tracker (DataStream SDK) at hostname %s", tracker_hostname.c_str());
// Make the ROS publisher
ros::Publisher mocap_pub = nh.advertise<lightweight_vicon_bridge::MocapMarkerArray>(tracker_topic, 1, false);
// Initialize the DataStream SDK
ViconDataStreamSDK::CPP::Client sdk_client;
ROS_INFO("Connecting to server...");
sdk_client.Connect(tracker_hostname);
usleep(10000);
while (!sdk_client.IsConnected().Connected && ros::ok())
{
ROS_WARN("...taking a while to connect, trying again...");
sdk_client.Connect(tracker_hostname);
usleep(10000);
}
ROS_INFO("...connected!");
// Enable data
sdk_client.EnableUnlabeledMarkerData();
// Set the axes (right-handed, X-forwards, Y-left, Z-up, same as ROS)
sdk_client.SetAxisMapping(ViconDataStreamSDK::CPP::Direction::Forward, ViconDataStreamSDK::CPP::Direction::Left, ViconDataStreamSDK::CPP::Direction::Up);
// Set streaming mode
if (stream_mode == "ServerPush")
{
sdk_client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ServerPush);
}
else if (stream_mode == "ClientPullPreFetch")
{
sdk_client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch);
}
else if (stream_mode == "ClientPull")
{
sdk_client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ClientPull);
}
else
{
ROS_FATAL("Invalid stream mode");
exit(-2);
}
// Start streaming data
ROS_INFO("Streaming data...");
while (ros::ok())
{
// Get a new frame and process it
if (sdk_client.GetFrame().Result == ViconDataStreamSDK::CPP::Result::Success)
{
double total_latency = sdk_client.GetLatencyTotal().Total;
ros::Duration latency_duration(total_latency);
ros::Time current_time = ros::Time::now();
ros::Time frame_time = current_time - latency_duration;
lightweight_vicon_bridge::MocapMarkerArray state_msg;
state_msg.header.frame_id = tracker_frame_name;
state_msg.header.stamp = frame_time;
state_msg.tracker_name = tracker_name;
// Get the unlabeled markers
unsigned int unlabelled_markers = sdk_client.GetUnlabeledMarkerCount().MarkerCount;
for (unsigned int idx = 0; idx < unlabelled_markers; idx++)
{
ViconDataStreamSDK::CPP::Output_GetUnlabeledMarkerGlobalTranslation position = sdk_client.GetUnlabeledMarkerGlobalTranslation(idx);
lightweight_vicon_bridge::MocapMarker marker_msg;
marker_msg.index = idx;
marker_msg.position.x = position.Translation[0] * 0.001;
marker_msg.position.y = position.Translation[1] * 0.001;
marker_msg.position.z = position.Translation[2] * 0.001;
state_msg.markers.push_back(marker_msg);
}
mocap_pub.publish(state_msg);
}
// Handle ROS stuff
ros::spinOnce();
}
sdk_client.DisableUnlabeledMarkerData();
sdk_client.Disconnect();
return 0;
}
