Hi,
I didn't want to write publisher?so I just modify the client.cpp ,is it ok?
// vpColVector v_c(6); // camera velocity used to produce 6 simulated poses
// for (int i = 0; i < N; i++)
// {
// v_c = 0;
// if (i == 0)
// {
// // Initialize first poses
// cMo.buildFrom(0, 0, 0.5, 0, 0, 0); // z=0.5 m
// wMe.buildFrom(0, 0, 0, 0, 0, 0); // Id
// }
// else if (i == 1)
// v_c[3] = M_PI / 8;
// else if (i == 2)
// v_c[4] = M_PI / 8;
// else if (i == 3)
// v_c[5] = M_PI / 10;
// else if (i == 4)
// v_c[0] = 0.5;
// else if (i == 5)
// v_c[1] = 0.8;
// vpHomogeneousMatrix cMc; // camera displacement
// cMc = vpExponentialMap::direct(v_c); // Compute the camera displacement due to the velocity applied to the camera
// if (i > 0)
// {
// // From the camera displacement cMc, compute the wMe and cMo matrixes
// cMo = cMc.inverse() cMo;
// wMe = wMe eMc cMc eMc.inverse();
jingxizc
commented
6 years ago
Hi, I didn't want to write publisher?so I just modify the client.cpp ,is it ok? // vpColVector v_c(6); // camera velocity used to produce 6 simulated poses // for (int i = 0; i < N; i++) // { // v_c = 0; // if (i == 0) // { // // Initialize first poses // cMo.buildFrom(0, 0, 0.5, 0, 0, 0); // z=0.5 m // wMe.buildFrom(0, 0, 0, 0, 0, 0); // Id // } // else if (i == 1) // v_c[3] = M_PI / 8; // else if (i == 2) // v_c[4] = M_PI / 8; // else if (i == 3) // v_c[5] = M_PI / 10; // else if (i == 4) // v_c[0] = 0.5; // else if (i == 5) // v_c[1] = 0.8;
// vpHomogeneousMatrix cMc; // camera displacement // cMc = vpExponentialMap::direct(v_c); // Compute the camera displacement due to the velocity applied to the camera // if (i > 0) // { // // From the camera displacement cMc, compute the wMe and cMo matrixes // cMo = cMc.inverse() cMo; // wMe = wMe eMc cMc eMc.inverse();
// } // geometry_msgs::Transform pose_c_o; // pose_c_o = visp_bridge::toGeometryMsgsTransform(cMo); // geometry_msgs::Transform pose_w_e; // pose_w_e = visp_bridge::toGeometryMsgsTransform(wMe); // camera_objectpublisher.publish(pose_c_o); // world_effectorpublisher.publish(pose_w_e); // emc_quick_comm.request.camera_object.transforms.push_back(pose_c_o); // emc_quick_comm.request.world_effector.transforms.push_back(pose_w_e);
// } geometry_msgs::Transform pose_c_o; geometry_msgs::Transform pose_w_e;
pose_c_o .translation.x= -0.2352; pose_c_o .translation.y= -0.4028; pose_c_o .translation.z= 1.0600; pose_c_o .rotation.x= 0.000990816769666054; pose_c_o .rotation.y= -0.339129557981157; pose_c_o .rotation.z= 0.832646383526645; pose_c_o .rotation.w= -0.437820913190016;
pose_w_e .translation.x=-0.440312403375319; pose_w_e .translation.y=0.193086003862725; pose_w_e .translation.z=-0.269996674854836; pose_w_e .rotation.x=0.35248; pose_w_e .rotation.y=-0.13503; pose_w_e .rotation.z=0.9258; pose_w_e .rotation.w=0.020663;
camera_objectpublisher.publish(pose_c_o); world_effectorpublisher.publish(pose_w_e); emc_quick_comm.request.camera_object.transforms.push_back(pose_c_o); emc_quick_comm.request.world_effector.transforms.push_back(pose_w_e);
pose_c_o .translation.x= 0.2019; pose_c_o .translation.y= -0.1199; pose_c_o .translation.z= 0.7585; pose_c_o .rotation.x= 0.144595808799497; pose_c_o .rotation.y= -0.160463581297842; pose_c_o .rotation.z= 0.853932081608368; pose_c_o .rotation.w=-0.473456857644354;
pose_w_e .translation.x=-0.595595551756106; pose_w_e .translation.y=0.0267552735221165; pose_w_e .translation.z=0.212462952686786; pose_w_e .rotation.x=0.33571; pose_w_e .rotation.y=-0.31476; pose_w_e .rotation.z=0.87863; pose_w_e .rotation.w=-0.12741;
camera_objectpublisher.publish(pose_c_o); world_effectorpublisher.publish(pose_w_e); emc_quick_comm.request.camera_object.transforms.push_back(pose_c_o); emc_quick_comm.request.world_effector.transforms.push_back(pose_w_e);