/**
* @brief DiagonalMatrix creates a diagonal matrix.
* @param D a vector of size 3.
* @return It returns a diagonal matrix.
*/
Eigen::Matrix3d DiagonalMatrix(Eigen::Vector3d D)
{
Eigen::Matrix3d ret;
ret.setZero();
ret(0, 0) = D[0];
ret(1, 1) = D[1];
ret(2, 2) = D[2];
return ret;
}
SRBASolver::SRBASolver()
{
rba_.setVerbosityLevel( 2 ); // 0: None; 1:Important only; 2:Verbose
// =========== Topology parameters ===========
rba_.parameters.srba.max_tree_depth = 3;
rba_.parameters.srba.max_optimize_depth = 3;
rba_.parameters.ecp.min_obs_to_loop_closure = 1; // This is a VERY IMPORTANT PARAM, if it is set to 1 everything goes to shit
rba_.parameters.srba.use_robust_kernel = false;
rba_.parameters.srba.optimize_new_edges_alone = true;
rba_.parameters.srba.dumpToConsole();
first_keyframe_ = true;
curr_kf_id_ = 0;
marker_count_ = 0;
relative_map_frame_ = "relative_map";
global_map_frame_ = "global_map";
loop_closed_ = false;
// Information matrix for relative pose observations:
{
Eigen::Matrix3d ObsL;
ObsL.setZero();
ObsL(0,0) = 1/(STD_NOISE_XY*STD_NOISE_XY); // x
ObsL(1,1) = 1/(STD_NOISE_XY*STD_NOISE_XY); // y
ObsL(2,2) = 1/(STD_NOISE_YAW*STD_NOISE_YAW); // phi
// Set:
rba_.parameters.obs_noise.lambda = ObsL;
}
}
void mesh_core::Plane::leastSquaresGeneral(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d* average)
{
if (points.empty())
{
normal_ = Eigen::Vector3d(0,0,1);
d_ = 0;
if (average)
*average = Eigen::Vector3d::Zero();
return;
}
// find c, the average of the points
Eigen::Vector3d c;
c.setZero();
EigenSTL::vector_Vector3d::const_iterator p = points.begin();
EigenSTL::vector_Vector3d::const_iterator end = points.end();
for ( ; p != end ; ++p)
c += *p;
c *= 1.0/double(points.size());
// Find the matrix
Eigen::Matrix3d m;
m.setZero();
p = points.begin();
for ( ; p != end ; ++p)
{
Eigen::Vector3d cp = *p - c;
m(0,0) += cp.x() * cp.x();
m(1,0) += cp.x() * cp.y();
m(2,0) += cp.x() * cp.z();
m(1,1) += cp.y() * cp.y();
m(2,1) += cp.y() * cp.z();
m(2,2) += cp.z() * cp.z();
}
m(0,1) = m(1,0);
m(0,2) = m(2,0);
m(1,2) = m(2,1);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(m);
if (eigensolver.info() == Eigen::Success)
{
normal_ = eigensolver.eigenvectors().col(0);
normal_.normalize();
}
else
{
normal_ = Eigen::Vector3d(0,0,1);
}
d_ = -c.dot(normal_);
if (average)
*average = c;
}
void LoadEstimator::getCrossMatrix(const Eigen::Vector3d& input_vector, Eigen::Matrix3d& crossed_matrix)
{
crossed_matrix.setZero(3,3);
crossed_matrix(0,1) = - input_vector(2);
crossed_matrix(1,0) = input_vector(2);
crossed_matrix(0,2) = input_vector(1);
crossed_matrix(2,0) = - input_vector(1);
crossed_matrix(1,2) = - input_vector(0);
crossed_matrix(2,1) = input_vector(0);
}
void BoundingBox::computeOrientedBox(std::vector<Vertex>& vertices)
{
type = "Oriented";
orientedPoints.clear();
// compute mean
Eigen::Vector3d center;
center.setZero();
for (VertexCIter v = vertices.begin(); v != vertices.end(); v++) {
center += v->position;
}
center /= (double)vertices.size();
// adjust for mean and compute covariance
Eigen::Matrix3d covariance;
covariance.setZero();
for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
Eigen::Vector3d pAdg = v->position - center;
covariance += pAdg * pAdg.transpose();
}
covariance /= (double)vertices.size();
// compute eigenvectors for the covariance matrix
Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance);
Eigen::Matrix3d eigenVectors = solver.eigenvectors().real();
// project min and max points on each principal axis
double min1 = INFINITY, max1 = -INFINITY;
double min2 = INFINITY, max2 = -INFINITY;
double min3 = INFINITY, max3 = -INFINITY;
double d = 0.0;
eigenVectors.transpose();
for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
d = eigenVectors.row(0).dot(v->position);
if (min1 > d) min1 = d;
if (max1 < d) max1 = d;
d = eigenVectors.row(1).dot(v->position);
if (min2 > d) min2 = d;
if (max2 < d) max2 = d;
d = eigenVectors.row(2).dot(v->position);
if (min3 > d) min3 = d;
if (max3 < d) max3 = d;
}
// add points to vector
orientedPoints.push_back(eigenVectors.row(0) * min1);
orientedPoints.push_back(eigenVectors.row(0) * max1);
orientedPoints.push_back(eigenVectors.row(1) * min2);
orientedPoints.push_back(eigenVectors.row(1) * max2);
orientedPoints.push_back(eigenVectors.row(2) * min3);
orientedPoints.push_back(eigenVectors.row(2) * max3);
}
void BoundingBox::computeOrientedBox(std::vector<Eigen::Vector3d>& positions)
{
// compute mean
Eigen::Vector3d cm;
cm.setZero();
for (size_t i = 0; i < positions.size(); i++) {
cm += positions[i];
}
cm /= (double)positions.size();
// adjust for mean and compute covariance matrix
Eigen::Matrix3d covariance;
covariance.setZero();
for (size_t i = 0; i < positions.size(); i++) {
Eigen::Vector3d pAdg = positions[i] - cm;
covariance += pAdg * pAdg.transpose();
}
covariance /= (double)positions.size();
// compute eigenvectors for covariance matrix
Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance);
Eigen::Matrix3d eigenVectors = solver.eigenvectors().real();
// set axes
eigenVectors.transpose();
xAxis = eigenVectors.row(0);
yAxis = eigenVectors.row(1);
zAxis = eigenVectors.row(2);
// project min and max points on each principal axis
double min1 = INF, max1 = -INF;
double min2 = INF, max2 = -INF;
double min3 = INF, max3 = -INF;
double d = 0.0;
for (size_t i = 0; i < positions.size(); i++) {
d = xAxis.dot(positions[i]);
if (min1 > d) min1 = d;
if (max1 < d) max1 = d;
d = yAxis.dot(positions[i]);
if (min2 > d) min2 = d;
if (max2 < d) max2 = d;
d = zAxis.dot(positions[i]);
if (min3 > d) min3 = d;
if (max3 < d) max3 = d;
}
// set center and halflengths
center = (xAxis*(min1 + max1) + yAxis*(min2 + max2) + zAxis*(min3 + max3)) /2;
halfLx = (max1 - min1)/2; halfLy = (max2 - min2)/2; halfLz = (max3 - min3)/2;
}
void mesh_core::Plane::leastSquaresFast(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d* average)
{
Eigen::Matrix3d m;
Eigen::Vector3d b;
Eigen::Vector3d c;
m.setZero();
b.setZero();
c.setZero();
EigenSTL::vector_Vector3d::const_iterator p = points.begin();
EigenSTL::vector_Vector3d::const_iterator end = points.end();
for ( ; p != end ; ++p)
{
m(0,0) += p->x() * p->x();
m(1,0) += p->x() * p->y();
m(2,0) += p->x();
m(1,1) += p->y() * p->y();
m(2,1) += p->y();
b(0) += p->x() * p->z();
b(1) += p->y() * p->z();
b(2) += p->z();
c += *p;
}
m(0,1) = m(1,0);
m(0,2) = m(2,0);
m(1,2) = m(2,1);
m(2,2) = double(points.size());
c *= 1.0/double(points.size());
normal_ = m.colPivHouseholderQr().solve(b);
if (normal_.squaredNorm() > std::numeric_limits<double>::epsilon())
normal_.normalize();
d_ = -c.dot(normal_);
if (average)
*average = c;
}
int main(int argc, char**argv)
{
my_srba_t rba; // Create an empty RBA problem
// --------------------------------------------------------------------------------
// Set parameters
// --------------------------------------------------------------------------------
rba.setVerbosityLevel( 1 ); // 0: None; 1:Important only; 2:Verbose
rba.parameters.srba.use_robust_kernel = false;
//rba.parameters.srba.optimize_new_edges_alone = false; // skip optimizing new edges one by one? Relative graph-slam without landmarks should be robust enough, but just to make sure we can leave this to "true" (default)
// Information matrix for relative pose observations:
{
Eigen::Matrix3d ObsL;
ObsL.setZero();
ObsL(0,0) = 1/square(STD_NOISE_XY); // x
ObsL(1,1) = 1/square(STD_NOISE_XY); // y
ObsL(2,2) = 1/square(STD_NOISE_YAW); // phi
// Set:
rba.parameters.obs_noise.lambda = ObsL;
}
// =========== Topology parameters ===========
rba.parameters.srba.edge_creation_policy = mrpt::srba::ecpICRA2013;
rba.parameters.srba.max_tree_depth = 3;
rba.parameters.srba.max_optimize_depth = 3;
rba.parameters.srba.submap_size = 5;
rba.parameters.srba.min_obs_to_loop_closure = 1;
// ===========================================
// --------------------------------------------------------------------------------
// Dump parameters to console (for checking/debugging only)
// --------------------------------------------------------------------------------
cout << "RBA parameters:\n-----------------\n";
rba.parameters.srba.dumpToConsole();
#if MRPT_HAS_WXWIDGETS
mrpt::gui::CDisplayWindow3D win("RBA results",640,480);
#endif
// --------------------------------------------------------------------------------
// Process the dataset:
// --------------------------------------------------------------------------------
const size_t nObs = sizeof(dataset)/sizeof(dataset[0]);
size_t cur_kf = 0; // Start at keyframe #0 in the dataset
for (size_t obsIdx = 0; obsIdx<nObs; cur_kf++ /* move to next KF */ )
{
// Create list of observations for keyframe: "cur_kf"
my_srba_t::new_kf_observations_t list_obs;
// To emulate graph-SLAM, each keyframe MUST have exactly ONE fixed "fake landmark", representing its pose:
// ------------------------------------------------------------------------------------------------------------
{
my_srba_t::new_kf_observation_t obs_field;
obs_field.is_fixed = true;
obs_field.obs.feat_id = cur_kf; // Feature ID == keyframe ID
obs_field.obs.obs_data.x = 0; // Landmark values are actually ignored.
obs_field.obs.obs_data.y = 0;
obs_field.obs.obs_data.yaw = 0;
list_obs.push_back( obs_field );
}
// The rest "observations" are real observations of relative poses:
// -----------------------------------------------------------------
while ( dataset[obsIdx].current_kf == cur_kf && obsIdx<nObs )
{
my_srba_t::new_kf_observation_t obs_field;
obs_field.is_fixed = false; // "Landmarks" (relative poses) have unknown relative positions (i.e. treat them as unknowns to be estimated)
obs_field.is_unknown_with_init_val = false; // Ignored, since all observed "fake landmarks" already have an initialized value.
obs_field.obs.feat_id = dataset[obsIdx].observed_kf;
obs_field.obs.obs_data.x = dataset[obsIdx].x + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XY);
obs_field.obs.obs_data.y = dataset[obsIdx].y + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XY);
obs_field.obs.obs_data.yaw = dataset[obsIdx].yaw + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_YAW);
list_obs.push_back( obs_field );
obsIdx++; // Next dataset entry
}
// Here happens the main stuff: create Key-frames, build structures, run optimization, etc.
// ============================================================================================
my_srba_t::TNewKeyFrameInfo new_kf_info;
rba.define_new_keyframe(
list_obs, // Input observations for the new KF
new_kf_info, // Output info
true // Also run local optimization?
);
cout << "Created KF #" << new_kf_info.kf_id
<< " | # kf-to-kf edges created:" << new_kf_info.created_edge_ids.size() << endl
<< "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl
<< "-------------------------------------------------------" << endl;
// Display:
#if MRPT_HAS_WXWIDGETS
// --------------------------------------------------------------------------------
// Show 3D view of the resulting map:
// --------------------------------------------------------------------------------
my_srba_t::TOpenGLRepresentationOptions opengl_options;
mrpt::opengl::CSetOfObjectsPtr rba_3d = mrpt::opengl::CSetOfObjects::Create();
rba.build_opengl_representation(
new_kf_info.kf_id , // Root KF: the current (latest) KF
opengl_options, // Rendering options
rba_3d // Output scene
);
{
mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock();
scene->clear();
scene->insert(rba_3d);
win.unlockAccess3DScene();
}
win.repaint();
cout << "Press any key to continue.\n";
win.waitForKey();
#endif
} // end-for each dataset entry
// --------------------------------------------------------------------------------
// Saving RBA graph as a DOT file:
// --------------------------------------------------------------------------------
const string sFil = "graph.dot";
cout << "Saving final graph of KFs and LMs to: " << sFil << endl;
rba.save_graph_as_dot(sFil, true /* LMs=save */);
cout << "Done.\n";
return 0; // All ok
}
/**
* @brief estimateHomography estimates an homography matrix H between image 1 to image 2
* @param points0 is an array of points computed from image 1.
* @param points1 is an array of points computed from image 2.
* @return It returns the homography matrix H.
*/
PIC_INLINE Eigen::Matrix3d estimateHomography(std::vector< Eigen::Vector2f > &points0,
std::vector< Eigen::Vector2f > &points1)
{
Eigen::Matrix3d H;
if((points0.size() != points1.size()) || (points0.size() < 4)) {
H.setZero();
return H;
}
Eigen::Vector3f transform_0 = ComputeNormalizationTransform(points0);
Eigen::Vector3f transform_1 = ComputeNormalizationTransform(points1);
Eigen::Matrix3d mat_0 = getShiftScaleMatrix(transform_0);
Eigen::Matrix3d mat_1 = getShiftScaleMatrix(transform_1);
int n = int(points0.size());
Eigen::MatrixXd A(n * 2, 9);
//set up the linear system
for(int i = 0; i < n; i++) {
//transform coordinates for increasing stability of the system
Eigen::Vector2f p0 = points0[i];
Eigen::Vector2f p1 = points1[i];
p0[0] = (p0[0] - transform_0[0]) / transform_0[2];
p0[1] = (p0[1] - transform_0[1]) / transform_0[2];
p1[0] = (p1[0] - transform_1[0]) / transform_1[2];
p1[1] = (p1[1] - transform_1[1]) / transform_1[2];
int j = i * 2;
A(j, 0) = 0.0;
A(j, 1) = 0.0;
A(j, 2) = 0.0;
A(j, 3) = p0[0];
A(j, 4) = p0[1];
A(j, 5) = 1.0;
A(j, 6) = -p1[1] * p0[0];
A(j, 7) = -p1[1] * p0[1];
A(j, 8) = -p1[1];
j++;
A(j, 0) = p0[0];
A(j, 1) = p0[1];
A(j, 2) = 1.0;
A(j, 3) = 0.0;
A(j, 4) = 0.0;
A(j, 5) = 0.0;
A(j, 6) = -p1[0] * p0[0];
A(j, 7) = -p1[0] * p0[1];
A(j, 8) = -p1[0];
}
//solve the linear system
Eigen::JacobiSVD< Eigen::MatrixXd > svd(A, Eigen::ComputeFullV);
Eigen::MatrixXd V = svd.matrixV();
n = int(V.cols()) - 1;
//assign and transpose
H(0, 0) = V(0, n);
H(0, 1) = V(1, n);
H(0, 2) = V(2, n);
H(1, 0) = V(3, n);
H(1, 1) = V(4, n);
H(1, 2) = V(5, n);
H(2, 0) = V(6, n);
H(2, 1) = V(7, n);
H(2, 2) = V(8, n);
H = mat_1.inverse() * H * mat_0;
return H / H(2, 2);
}
void init(){
//find the path of config files
std::string selfpath = get_selfpath();
//select using normal control mode or psudogravity control mode
right_rmt = NormalMode;
//initialize FT sensor ptr
ft_gama = new gamaFT;
ft.setZero(6);
tool_vec_g.setZero();
axis_end_vec.setZero();
//show toolname
tn = hingedtool;
StopFlag = false;
//initialize the axis vec
local_hinged_axis_vec.setZero();
local_hinged_axis_vec(0) = -0.9472;
local_hinged_axis_vec(1) = 0.0494;
local_hinged_axis_vec(2) = -0.3166;
des_tm.setZero();
des_vec.setZero();
//declare the cb function
boost::function<void(boost::shared_ptr<std::string>)> button_sdh_moveto(sdh_moveto_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhaxisvec_moveto(sdhaxisvec_moveto_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhmaintainF(sdhmaintainF_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhslideX(sdhslideX_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhslideY(sdhslideY_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhfoldtool(sdhfoldtool_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_gamaftcalib(gamaftcalib_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdh_grav_comp_ctrl(sdh_grav_comp_ctrl_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdh_normal_ctrl(sdh_normal_ctrl_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_brake(brake_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_nobrake(nobrake_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_closeprog(closeprog_cb);
//specify controller configure file in order to load it(right kuka + shadow hand)
std::string config_filename = selfpath + "/etc/right_arm_param.xml";
std::cout<<"right arm config file name is: "<<config_filename<<std::endl;
//load controller parameters
right_pm = new ParameterManager(config_filename);
//initialize ptr to right kuka com okc
com_okc_right = new ComOkc(kuka_right,OKC_HOST,OKC_PORT);
//connect kuka right
com_okc_right->connect();
//initialize the kuka robot and let it stay in the init pose
kuka_right_arm = new KukaLwr(kuka_right,*com_okc_right,tn);
//initialize the robot state
right_rs = new RobotState(kuka_right_arm);
//get the initialize state of kuka right
kuka_right_arm->get_joint_position_act();
kuka_right_arm->update_robot_state();
right_rs->updated(kuka_right_arm);
right_ac_vec.push_back(new ProActController(*right_pm));
right_task_vec.push_back(new KukaSelfCtrlTask(RP_NOCONTROL));
right_task_vec.back()->mt = JOINTS;
right_task_vec.back()->mft = GLOBAL;
Eigen::Vector3d p,o;
p.setZero();
o.setZero();
//get start point position in cartesian space
p(0) = initP_x = right_rs->robot_position["eef"](0);
p(1) = initP_y= right_rs->robot_position["eef"](1);
p(2) = initP_z= right_rs->robot_position["eef"](2);
o = tm2axisangle(right_rs->robot_orien["eef"]);
initO_x = o(0);
initO_y = o(1);
initO_z = o(2);
right_task_vec.back()->set_desired_p_eigen(p);
right_task_vec.back()->set_desired_o_ax(o);
kuka_right_arm->setAxisStiffnessDamping(right_ac_vec.back()->pm.stiff_ctrlpara.axis_stiffness, \
right_ac_vec.back()->pm.stiff_ctrlpara.axis_damping);
com_rsb = new ComRSB();
rdtschunkjs = SchunkJS;
com_rsb->add_msg(rdtschunkjs);
gama_f_filter = new TemporalSmoothingFilter<Eigen::Vector3d>(10,Average,Eigen::Vector3d(0,0,0));
gama_t_filter = new TemporalSmoothingFilter<Eigen::Vector3d>(10,Average,Eigen::Vector3d(0,0,0));
//register cb function
com_rsb->register_external("/foo/sdhmoveto",button_sdh_moveto);
com_rsb->register_external("/foo/sdhaxisvecmoveto",button_sdhaxisvec_moveto);
com_rsb->register_external("/foo/sdhmaintainF",button_sdhmaintainF);
com_rsb->register_external("/foo/sdhslideX",button_sdhslideX);
com_rsb->register_external("/foo/sdhslideY",button_sdhslideY);
com_rsb->register_external("/foo/sdhfoldtool",button_sdhfoldtool);
com_rsb->register_external("/foo/gamaftcalib",button_gamaftcalib);
com_rsb->register_external("/foo/sdh_grav_comp_ctrl",button_sdh_grav_comp_ctrl);
com_rsb->register_external("/foo/sdh_normal_ctrl",button_sdh_normal_ctrl);
com_rsb->register_external("/foo/closeprog",button_closeprog);
#ifdef HAVE_ROS
std::string left_kuka_arm_name="la";
std::string right_kuka_arm_name="ra";
std::string left_schunk_hand_name ="lh";
js_la.name.push_back(left_kuka_arm_name+"_arm_0_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_1_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_2_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_3_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_4_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_5_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_6_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_0_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_1_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_2_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_3_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_4_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_5_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_6_joint");
//for schunk
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_knuckle_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_finger_22_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_finger_23_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_thumb_2_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_thumb_3_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_finger_12_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_finger_13_joint");
js_la.position.resize(7);
js_la.velocity.resize(7);
js_la.effort.resize(7);
js_ra.position.resize(7);
js_ra.velocity.resize(7);
js_ra.effort.resize(7);
js_schunk.position.resize(7);
js_schunk.velocity.resize(7);
js_schunk.effort.resize(7);
js_la.header.frame_id="frame_la";
js_ra.header.frame_id="frame_ra";
js_ra.header.frame_id="frame_lh";
gamma_force_marker_pub = nh->advertise<visualization_msgs::Marker>("gamma_force_marker", 2);
hingedtool_axis_marker_pub = nh->advertise<visualization_msgs::Marker>("hingedtool_axis_marker", 2);
jsPub_la = nh->advertise<sensor_msgs::JointState> ("/la/joint_states", 2);
jsPub_ra = nh->advertise<sensor_msgs::JointState> ("/ra/joint_states", 2);
jsPub_schunk = nh->advertise<sensor_msgs::JointState> ("/lh/joint_states", 2);
ros::spinOnce();
br = new tf::TransformBroadcaster();
std::cout<<"ros init finished"<<std::endl;
#endif
}