void Network::sendScantoMap(gtsam::Pose3 pose, sensor_msgs::PointCloud2 scan) {
mapPub.publish(scan);
sensor_msgs::PointCloud::Ptr scan_body(new sensor_msgs::PointCloud);
sensor_msgs::convertPointCloud2ToPointCloud(scan, *scan_body);
//mapPub.publish(scan_body);
gtsam::Vector3 rpy = pose.rotation().rpy();
tf::Quaternion q;
q.setRPY(rpy(0), rpy(1), rpy(2));
geometry_msgs::Quaternion qmsg;
tf::quaternionTFToMsg(q,qmsg);
geometry_msgs::Pose pose_msg;
pose_msg.orientation = qmsg;
pose_msg.position.x = pose.x();
pose_msg.position.y = pose.y();
pose_msg.position.z = pose.z();
geometry_msgs::PoseStamped stamped_pose;
stamped_pose.pose = pose_msg;
stamped_pose.header.frame_id = "world";
stamped_pose.header.stamp = ros::Time::now();
posePub.publish(stamped_pose);
dvm_.addRegisteredPointCloud(pose_msg,scan_body,range_max_);
}
VectorXd MotionModel::tr2rpy(MatrixXd m)
{
VectorXd rpy(3);
if (abs(m(0,0)) < eps && abs(m(1,0)) < eps)
rpy << 0, atan2(-m(2,0), m(0,0)), atan2(-m(1,2), m(1,1));
else
{
double sp, cp;
sp = sin(atan2(m(1,0), m(0,0)));
cp = cos(atan2(m(1,0), m(0,0)));
rpy << atan2(m(1,0), m(0,0)), atan2(-m(2,0), cp * m(0,0) + sp * m(1,0)),
atan2(sp * m(0,2) - cp * m(1,2), cp * m(1,1) - sp * m(0,1));
}
return rpy;
}
int main(int argc,char** argv){
ww::World_wrapper world_wrapper;
geo::fCVec3 origin_ = {{0,0,-0.02/2 - 0.03/2}};
geo::fCVec3 dim_ = {{0.8,0.4,0.05}};
geo::fCVec3 orientation_ = {{M_PI/2,0,M_PI/2}};
wobj::WBox wsocket_wall("socket_wall",dim_,origin_,orientation_);
/// add a socket
tf::Vector3 origin(0,0,0);
tf::Vector3 rpy(M_PI/2,0,M_PI/2);
obj::Socket_one socket_one("socket_one",origin,rpy,1);
world_wrapper.wrapped_objects.push_back_box(wsocket_wall);
world_wrapper.wrapped_objects.push_back_socket(socket_one.wsocket);
world_wrapper.wrapped_objects.push_back_box(socket_one.wbox);
plugfilter::PF_parameters pf_parameters(world_wrapper.wrapped_objects);
pf_parameters.number_particles = 4000;
Sensor_parameters sensor_parameters(world_wrapper.wrapped_objects);
sensor_parameters.t_sensor = psm::SIMPLE_CONTACT_DIST;
Run_trajectories run_trajectories;
// run_trajectories.setup(pf_parameters,sensor_parameters);
std::string folder_name = "/home/guillaume/MatlabWorkSpace/peg_in_hole/TextData";
std::string subject_name = "Albert";
std::string plug_type = "A";
std::size_t i = 1;
std::string folder_path = folder_name + "/" + subject_name + "/";
std::string file_name = subject_name + "_" + plug_type + "_" + boost::lexical_cast<std::string>(i) + "_.txt";
run_trajectories.load(folder_path,file_name);
return 0;
}
void Peg_world_wrapper::initialise_socket(const std::string& socket_link_name,const std::string& wall_link_name){
tf::StampedTransform transform;
opti_rviz::Listener::get_tf_once(fixed_frame,socket_link_name,transform);
opti_rviz::Listener::print(transform);
/// add a socket
tf::Vector3 origin = transform.getOrigin();
tf::Vector3 rpy(M_PI/2,0,M_PI/2);
socket_one = obj::Socket_one(socket_link_name,wall_link_name,origin,rpy,1);
world_wrapper.wrapped_objects.push_back_box(&(socket_one.wbox));
world_wrapper.wrapped_objects.push_back_socket(socket_one.wsocket);
world_wrapper.wrapped_objects.push_back_box(&(socket_one.hole_wboxes[0]));
world_wrapper.wrapped_objects.push_back_box(&(socket_one.hole_wboxes[1]));
world_wrapper.wrapped_objects.push_back_box(&(socket_one.hole_wboxes[2]));
}
static inline std::vector<YAML::Node> get_frame_tip_nodes(const KDL::Segment& seg)
{
std::vector<YAML::Node> frames;
KDL::Frame frame = seg.getFrameToTip() * seg.getJoint().pose(0).Inverse();
// Set xyz
if(!KDL::Equal(frame.p, KDL::Vector::Zero()))
{
YAML::Node xyz_frame;
YAML::Node axis_angle_null;
axis_angle_null["axis-angle"].push_back(get_vector3(1, 0, 0));
axis_angle_null["axis-angle"].push_back(0);
xyz_frame["frame"].push_back(axis_angle_null);
xyz_frame["frame"].push_back(get_vector3(frame.p));
frames.push_back(xyz_frame);
}
// Set rpy
if(!KDL::Equal(frame.M, KDL::Rotation::Identity()))
{
std::vector<double> rpy(3);
frame.M.GetRPY(rpy[0],rpy[1],rpy[2]);
for (std::vector<double>::size_type i = rpy.size() - 1;
i != (std::vector<double>::size_type) -1; i--)
{
if(rpy[i] != 0)
{
YAML::Node rpy_frame;
YAML::Node axis_angle;
KDL::Vector axis;
axis[i] = 1;
axis_angle["axis-angle"].push_back(get_vector3(axis));
axis_angle["axis-angle"].push_back(rpy[i]);
rpy_frame["frame"].push_back(axis_angle);
rpy_frame["frame"].push_back(get_vector3(0, 0, 0));
frames.push_back(rpy_frame);
}
}
}
return frames;
}
int main(int argc, char* argv[])
{
ros::init(argc,argv,"navsts2odom");
ros::NodeHandle nh, ph("~");
//Get rotation between the two
std::vector<double> rpy(3,0);
ph.param("rpy",rpy,rpy);
//Setup the LTP to Odom frame
Eigen::Quaternion<double> q;
labust::tools::quaternionFromEulerZYX(rpy[0], rpy[1], rpy[2],q);
Eigen::Matrix3d rot = q.toRotationMatrix().transpose();
ros::Publisher odom = nh.advertise<nav_msgs::Odometry>("uwsim_hook", 1);
ros::Subscriber navsts = nh.subscribe<auv_msgs::NavSts>("navsts",1,
boost::bind(&onNavSts, boost::ref(odom), boost::ref(rot), _1));
ros::spin();
return 0;
}
void transformCam2Robot (const pcl::PointCloud<pcl::PointXYZRGB> &cloud_in, pcl::PointCloud<pcl::PointXYZRGB> &cloud_out)
{
if (&cloud_in != &cloud_out) {
cloud_out = cloud_in;
}
// Sensor settings
Eigen::Vector3d rpy(0, 0.18, 3.14);
Eigen::Vector4d xyz(mobile_base_pose.position.x, mobile_base_pose.position.y, mobile_base_pose.position.z, 1);
Eigen::Matrix3d R;
tf::Quaternion qt = tf::createQuaternionFromRPY(-rpy[1],-rpy[0],-rpy[2]);
tf::Matrix3x3 R1(qt);
tf::matrixTFToEigen(R1,R);
Eigen::Matrix4d tf_matrix;
tf_matrix.setZero();
tf_matrix.block (0, 0, 3, 3) = R;
tf_matrix (3, 3) = 1;
Eigen::Matrix4d tf_matrix_swap;
tf_matrix_swap << 0, 1, 0, 0,
0, 0, 1, 0,
1, 0, 0, 0,
0, 0, 0, 1;
double roll, pitch, yaw;
tf::Quaternion q(
mobile_base_pose.orientation.x,
mobile_base_pose.orientation.y,
mobile_base_pose.orientation.z,
mobile_base_pose.orientation.w
);
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
tf::Quaternion qt2 = tf::createQuaternionFromRPY(-rpy[1], -yaw, M_PI);
tf::Matrix3x3 R2(qt2);
tf::matrixTFToEigen(R2,R);
Eigen::Matrix4d tf_final;
tf_final.setZero();
tf_final.block (0, 0, 3, 3) << tf_matrix.block (0, 0, 3, 3) * R;
//tf_matrix.block (0, 0, 3, 3) = R;//.transpose();
tf_final.block (0, 3, 4, 1) << tf_matrix * tf_matrix_swap * xyz;
//tf_matrix (3, 3) = 1;
//tf_final = tf_matrix * tf_matrix_swap * tf_final;
// Perfrom transformation
for (size_t i = 0; i < cloud_in.points.size (); i++)
{
Eigen::Vector4d pt(cloud_in.points[i].x, cloud_in.points[i].y, cloud_in.points[i].z, 1);
Eigen::Vector4d pt_tf = tf_final*pt;
cloud_out.points[i].x = pt_tf[0];
cloud_out.points[i].y = pt_tf[1];
cloud_out.points[i].z = pt_tf[2];
}
}
//From Rotation Matrix, find rpy
Vector3f rollpy(MatrixXd R, facemidpts facepoints)
{
float sp, cp;
MatrixXf Rf = R.cast<float>();
ros::Rate loop_rate(30); //publish at 30Hz*/
geometry_msgs::Twist posemsg;
while(ros::ok())
{
if (abs(R(0,0)) < .001 & abs(R(1,0)) < .001)
{
// singularity
rpy(0) = 0;
rpy(1) = atan2(-Rf(2,0), Rf(0,0));
rpy(2) = atan2(-Rf(1,2), Rf(1,1));
posemsg.linear.x = facepoints.x;
posemsg.linear.y = facepoints.y;
posemsg.linear.z = facepoints.z;
posemsg.angular.x = rpy(0);
posemsg.angular.y = rpy(1);
posemsg.angular.z = rpy(2);
// ROS_INFO_STREAM("Head Translation: " << posemsg.linear);
// ROS_INFO_STREAM("Head RPY Angles: " << posemsg.angular);
boost::asio::io_service io_service;
const std::string multicast_address = "235.255.0.1";
sender s(io_service, boost::asio::ip::address::from_string(multicast_address), posemsg.linear.x, \
posemsg.linear.y, posemsg.linear.z, posemsg.angular.x, \
posemsg.angular.y, posemsg.angular.z);
io_service.run();
loop_rate.sleep();
}
else
{
rpy(0) = atan2(Rf(1,0), Rf(0,0));
sp = sin(rpy(0));
cp = cos(rpy(0));
rpy(1) = atan2(-Rf(2,0), cp * Rf(0,0) + sp * Rf(1,0));
rpy(2) = atan2(sp * Rf(0,2) - cp * Rf(1,2), cp*Rf(1,1) - sp*Rf(0,1));
posemsg.linear.x = facepoints.x;
posemsg.linear.y = facepoints.y;
posemsg.linear.z = facepoints.z;
posemsg.angular.x = rpy(0);
posemsg.angular.y = rpy(1);
posemsg.angular.z = rpy(2);
// ROS_INFO_STREAM("Head Translation: "<< posemsg.linear);
// ROS_INFO_STREAM("Head RPY Angles: "<< posemsg.angular);
boost::asio::io_service io_service;
const std::string multicast_address = "235.255.0.1";
sender s(io_service, boost::asio::ip::address::from_string(multicast_address), posemsg.linear.x, \
posemsg.linear.y, posemsg.linear.z, posemsg.angular.x, \
posemsg.angular.y, posemsg.angular.z);
io_service.run();
loop_rate.sleep();
}
//ros::waitForShutdown();
}
return rpy;
}
int main(int argc, char **argv){
ros::init(argc, argv, "automap");
ros::NodeHandle nh;
// create dynamic reconfigure object
dynamic_reconfigure::Server<automap::ExplorationConfig> server;
dynamic_reconfigure::Server<automap::ExplorationConfig>::CallbackType f;
automap::ExplorationConfig dynConfig;
ros::Time initTime = ros::Time::now();
nav_msgs::MapMetaData mapMetaData;
cv::Mat map;
simulation::telemetry_msg telemetry;
sensor_msgs::ImagePtr edgeImageMsg;
MoveBaseClient ac("move_base", true);
while(!ac.waitForServer(ros::Duration(5.0)) && ros::ok())
{
ROS_INFO("Waiting for the move_base action server to come up...");
}
//control_On : motion control on/off / detection_On : sensing on/off / NBV_On : nbv on/off
automap::automap_ctrl_msg ctrlSignals;
tf::TransformListener listener;
geometry_msgs::Pose position;
std::vector<double> rpy(3, 0.0);
cv::Point gridPose;
ros::Subscriber mapMetaSub = nh.subscribe<nav_msgs::MapMetaData>("map_metadata", 10, boost::bind(mapMetaCallback, _1, &mapMetaData));
ros::Subscriber ctrlSub = nh.subscribe<automap::automap_ctrl_msg>("automap/ctrl_msg", 10, boost::bind(ctrlCallback, _1, &ctrlSignals));
ros::Subscriber mapSub = nh.subscribe<nav_msgs::OccupancyGrid>("map", 10, boost::bind(mapCallback, _1, &map));
ros::Publisher pathPub = nh.advertise<nav_msgs::Path>("pathtransformPlanner/path", 10);
ros::Subscriber robotInfo = nh.subscribe<simulation::telemetry_msg>("telemetry", 10, boost::bind(telemetryCallback, _1, &telemetry));
image_transport::ImageTransport it(nh);
image_transport::Publisher edgePub = it.advertise("floatingEdges", 1);
//wait for map - server
ros::Duration d = ros::Duration(2, 0);
ros::spinOnce();
while(mapMetaData.resolution == 0 && ros::ok()) {
ROS_INFO("Waiting for the map server to come up...");
d.sleep();
ros::spinOnce();
}
while(map.cols==0 && map.rows==0 && ros::ok()) {
ROS_INFO("Waiting for the map server to come up...");
d.sleep();
ros::spinOnce();
}
PathtransformPlanner pPlanner(mapMetaData);
ExplorationPlanner ePlanner(&pPlanner);
// Begin: exploration metrics
// array for drawing the robots path
std::vector<cv::Point> explorationPath;
// average planner time
ros::Time planner_timer;
double t_planner = 0;
unsigned int t_planner_counter = 0;
// average ex-planner time
ros::Time explanner_timer;
double t_explanner = 0;
unsigned int t_explanner_counter = 0;
// average busy loop time
ros::Time loop_timer;
double t_loop = 0;
unsigned int t_loop_counter = 0;
double timediff = 0;
f = boost::bind(&configCallback, _1, _2, &pPlanner, &ePlanner, &dynConfig);
server.setCallback(f);
ctrlSignals.control_On=dynConfig.node_control_on;
ctrlSignals.detection_On=dynConfig.node_sensing_on;
ctrlSignals.NBV_On=dynConfig.node_use_nbv;
bool finished = false;
bool finalCheck = false;
int retry = dynConfig.node_retries;
cv::Mat old = cv::Mat::zeros(map.rows, map.cols, CV_8UC1);
// Loop starts here:
ros::Rate loop_rate(dynConfig.node_loop_rate);
while(ros::ok() && !finished) {
//start measure processing time
loop_timer = ros::Time::now();
//calculate information gain
double gain = cv::norm(old, map, CV_L2);
//Get Position Information...
getPositionInfo("map", "base_footprint", listener, &position, &rpy);
setGridPosition(position, mapMetaData, &gridPose);
//Remember path
explorationPath.push_back(gridPose);
if(gain>dynConfig.node_information_gain || retry==0) {
ROS_INFO("Enough information gained: %lf",gain);
old = map.clone();
retry = dynConfig.node_retries;
//Feed pathtransformPlanner...
try{
ROS_INFO("Feeding PathtransformPlanner...");
//start measure processing time
planner_timer = ros::Time::now();
pPlanner.updateTransformMatrices(map, gridPose);
//stop measure processing time
timediff = (ros::Time::now()-planner_timer).toNSec()/NSEC_PER_SEC;
t_planner += timediff;
t_planner_counter++;
ROS_INFO("Path planning took: %lf",timediff);
}catch(std::exception& e) {
std::cout<<e.what()<<std::endl;
}
bool w = false;
if(ctrlSignals.detection_On) {
//start measure processing time
explanner_timer = ros::Time::now();
w = ePlanner.findBestPlan(map, gridPose, rpy[2], ctrlSignals.NBV_On);
//stop measure processing time
timediff = (ros::Time::now()-explanner_timer).toNSec()/NSEC_PER_SEC;
t_explanner += timediff;
t_explanner_counter++;
ROS_INFO("Exploration planning took: %lf",timediff);
if(!w && !finalCheck){
finalCheck = true;
continue;
}
}else{
w = true;
}
if(w) {
finalCheck =false;
ROS_INFO("Best next Plan found!");
std_msgs::Header genericHeader;
genericHeader.stamp = ros::Time::now();
genericHeader.frame_id = "map";
// send map with valid detected Edges
if(dynConfig.node_show_exploration_planner_result){
cv::Mat out = ePlanner.drawFrontiers();
edgeImageMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", out).toImageMsg();
edgePub.publish(edgeImageMsg);
}
nav_msgs::Path frontierPath;
if(ctrlSignals.detection_On) {
frontierPath = ePlanner.getBestPlan(genericHeader);
}
if(ctrlSignals.control_On && ctrlSignals.detection_On) {
ROS_INFO("Sending Plan..");
pathPub.publish(frontierPath);
ros::spinOnce();
ROS_INFO("Sending Goal..");
sendGoal(frontierPath, ac);
}
}else{
ROS_INFO("Map exploration finished, aborting loop...");
//ac.waitForResult();
ac.cancelGoal();
ac.waitForResult();
std::string imgMetaPath = ros::package::getPath("automap") + "/data/output/map.yaml";
std::string imgStatPath = ros::package::getPath("automap") + "/data/output/exploration_statistics.txt";
std::string imgPath = ros::package::getPath("automap") + "/data/output/map.pgm";
std::string recordedPathPath = ros::package::getPath("automap") + "/data/output/path.png";
//Save explored map
std::vector<int> com_param;
com_param.push_back(CV_IMWRITE_PNG_COMPRESSION);
com_param.push_back(9);
try {
cv::imwrite(imgPath, map, com_param);
ROS_INFO("Map written to: %s", imgPath.c_str());
} catch (std::runtime_error& ex) {
std::cout << "Exception converting img to PNG: " << ex.what() << std::endl;
}
//Save exploration path
cv::Mat pathMap = map.clone();
cv::cvtColor(pathMap, pathMap, CV_GRAY2RGB);
cv::polylines(pathMap, explorationPath, false, cv::Scalar(0, 0, 255), 1, 8);
try {
cv::imwrite(recordedPathPath, pathMap, com_param);
ROS_INFO("Path written to: %s", recordedPathPath.c_str());
} catch (std::runtime_error& ex) {
std::cout << "Exception converting img to PNG: " << ex.what() << std::endl;
}
YAML::Emitter Y_out;
Y_out << YAML::BeginMap;
Y_out << YAML::Key << "image";
Y_out << YAML::Value << "map.pgm";
Y_out << YAML::Key << "resolution";
Y_out << YAML::Value << mapMetaData.resolution;
Y_out << YAML::Key << "origin";
Y_out << YAML::Flow;
Y_out << YAML::BeginSeq << mapMetaData.origin.position.x<<mapMetaData.origin.position.y
<<mapMetaData.origin.position.z << YAML::EndSeq;
Y_out << YAML::Key << "negate";
Y_out << YAML::Value << 0;
Y_out << YAML::Key << "occupied_thresh";
Y_out << YAML::Value << 0.65;
Y_out << YAML::Key << "free_thresh";
Y_out << YAML::Value << 0.196;
Y_out << YAML::EndMap;
YAML::Emitter Y_out_statistics;
Y_out_statistics << YAML::BeginMap;
Y_out_statistics << YAML::Key << "driven distance during exploration(m)";
Y_out_statistics << YAML::Value << telemetry.radial_distance;
Y_out_statistics << YAML::Key << "time elapsed during exploration(s)";
Y_out_statistics << YAML::Value << (ros::Time::now()-initTime).toNSec()/NSEC_PER_SEC;
Y_out_statistics << YAML::Key << "average path planner processing time(s)";
Y_out_statistics << YAML::Value << t_planner/t_planner_counter;
Y_out_statistics << YAML::Key << "average exploration planner processing time(s)";
Y_out_statistics << YAML::Value << t_explanner/t_explanner_counter;
Y_out_statistics << YAML::Key << "average busy loop processing time(s)";
Y_out_statistics << YAML::Value << t_loop/t_loop_counter;
// calc quality of map
std::string godMapPath = ros::package::getPath("simulation") + "/data/map/map.pgm";
cv::Mat godMap = cv::imread(godMapPath,1);
cv::cvtColor(godMap, godMap, CV_RGB2GRAY);
double diffMap = cv::norm(godMap, map, CV_L2);
Y_out_statistics << YAML::Key << "in comparison to god map (L2Norm)";
Y_out_statistics << YAML::Value << diffMap;
Y_out_statistics << YAML::EndMap;
std::ofstream outfile_yaml(imgMetaPath);
try{
outfile_yaml<<Y_out.c_str();
ROS_INFO("MapMetaData written to: %s", imgMetaPath.c_str());
}catch (std::runtime_error& ex) {
std::cout << "Exception writing .yaml-file: " << ex.what() << std::endl;
}
outfile_yaml.close();
std::ofstream outfile_statistics(imgStatPath);
try{
outfile_statistics<<Y_out_statistics.c_str();
ROS_INFO("Statistics data written to: %s", imgStatPath.c_str());
}catch (std::runtime_error& ex) {
std::cout << "Exception writing .txt-file: " << ex.what() << std::endl;
}
outfile_statistics.close();
finished = true;
ros::shutdown();
}
}else{
ROS_INFO("Not enough information gained: %lf",gain);
retry--;
}
// resend map with valid detected Edges
if(dynConfig.node_show_exploration_planner_result){
cv::Mat out = ePlanner.drawFrontiers();
edgeImageMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", out).toImageMsg();
edgePub.publish(edgeImageMsg);
}
//stop measure processing time
timediff = (ros::Time::now()-loop_timer).toNSec()/NSEC_PER_SEC;
t_loop += timediff;
t_loop_counter++;
ROS_INFO("Whole loop took: %lf",timediff);
ros::spinOnce();
loop_rate.sleep();
}
ros::spin();
}
int main(int argc, char** argv){
std::map<std::string,std::string> input;
input["-traj_path"] = "/home/guillaume/MatlabWorkSpace/peg_in_hole/TextData/Albert/";
input["-rate"] = "10";
input["-broadcast_plug"] = "plug_link";
input["-broadcast_socket"] = "link_socket";
if(!opti_rviz::Input::process_input(argc,argv,input)){
ROS_ERROR("failed to load input");
return 0;
}
opti_rviz::Input::print_input_options(input);
ros::init(argc, argv, "peg_in_hole_replay_filter");
ros::NodeHandle node;
//ros::Rate rate(boost::lexical_cast<float>(input["-rate"])); // initialise world
ros::Rate rate(100);
/// Wrap the world
ww::World_wrapper world_wrapper;
geo::fCVec3 origin_ = {{0,0,-0.02/2 - 0.03/2}};
geo::fCVec3 dim_ = {{0.8,0.4,0.05}};
geo::fCVec3 orientation_ = {{M_PI/2,0,M_PI/2}};
wobj::WBox wsocket_wall("socket_wall",dim_,origin_,orientation_);
tf::Vector3 origin(0,0,0);
tf::Vector3 rpy(M_PI/2,0,M_PI/2);
obj::Socket_one socket_one("socket_one",origin,rpy,1);
world_wrapper.wrapped_objects.push_back_box(wsocket_wall);
world_wrapper.wrapped_objects.push_back_socket(socket_one.wsocket);
world_wrapper.wrapped_objects.push_back_box(socket_one.wbox);
/// Filter and Sensor parameters
Sensor_parameters sensor_parameters(world_wrapper.wrapped_objects);
plugfilter::PF_parameters pf_parameters(world_wrapper.wrapped_objects);
pf_parameters.particle_filter_type = plugfilter::SIR;
pf_parameters.number_particles = 1500;
pf_parameters.likelihood_t = likeli::FOUR_CONTACT;
sensor_parameters.t_sensor = psm::FOUR_CONTACT_DIST;
sensor_parameters.b_print_sensor = true;
pf_parameters.visualisation_mode = opti_rviz::Vis_point_cloud::DEFAULT;
pf_parameters.pf_color_type = pf::C_WEIGHTS;
pf_parameters.sampling_parameters.set_diag(0.0005,0.01,0.01);
/// REPLAY
Peg_replay peg_replay(node,input["-broadcast_plug"],input["-broadcast_socket"]);
peg_replay.set_traj_dir_path(input["-traj_path"]);
peg_replay.run_trajectories.setup_pf(pf_parameters);
peg_replay.run_trajectories.setup_sensor(sensor_parameters);
peg_replay.initalise_vision(node);
peg_replay.use_contact_model(world_wrapper.wrapped_objects);
peg_replay.run_trajectories.add_reguliser();
/// Visualise features
ww::Publisher publisher("visualization_marker",&node,&world_wrapper);
publisher.init("/world");
publisher.update_position();
while(node.ok()){
peg_replay.update();
peg_replay.visualise();
publisher.publish();
ros::spinOnce();
rate.sleep();
}
return 0;
}
