void ConvexPolygon::projectOnPlane(
const Eigen::Affine3f& pose, Eigen::Affine3f& output)
{
Eigen::Vector3f p(pose.translation());
Eigen::Vector3f output_p;
projectOnPlane(p, output_p);
// ROS_INFO("[ConvexPolygon::projectOnPlane] p: [%f, %f, %f]",
// p[0], p[1], p[2]);
// ROS_INFO("[ConvexPolygon::projectOnPlane] output_p: [%f, %f, %f]",
// output_p[0], output_p[1], output_p[2]);
Eigen::Quaternionf rot;
rot.setFromTwoVectors(pose.rotation() * Eigen::Vector3f::UnitZ(),
coordinates().rotation() * Eigen::Vector3f::UnitZ());
Eigen::Quaternionf coords_rot(coordinates().rotation());
Eigen::Quaternionf pose_rot(pose.rotation());
// ROS_INFO("[ConvexPolygon::projectOnPlane] rot: [%f, %f, %f, %f]",
// rot.x(), rot.y(), rot.z(), rot.w());
// ROS_INFO("[ConvexPolygon::projectOnPlane] coords_rot: [%f, %f, %f, %f]",
// coords_rot.x(), coords_rot.y(), coords_rot.z(), coords_rot.w());
// ROS_INFO("[ConvexPolygon::projectOnPlane] pose_rot: [%f, %f, %f, %f]",
// pose_rot.x(), pose_rot.y(), pose_rot.z(), pose_rot.w());
// ROS_INFO("[ConvexPolygon::projectOnPlane] normal: [%f, %f, %f]", normal_[0], normal_[1], normal_[2]);
// Eigen::Affine3f::Identity() *
// output.translation() = Eigen::Translation3f(output_p);
// output.rotation() = rot * pose.rotation();
//output = Eigen::Translation3f(output_p) * rot * pose.rotation();
output = Eigen::Affine3f(rot * pose.rotation());
output.pretranslate(output_p);
// Eigen::Vector3f projected_point = output * Eigen::Vector3f(0, 0, 0);
// ROS_INFO("[ConvexPolygon::projectOnPlane] output: [%f, %f, %f]",
// projected_point[0], projected_point[1], projected_point[2]);
}
void UrdfModelMarker::addInvisibleMeshMarkerControl(visualization_msgs::InteractiveMarker &int_marker, boost::shared_ptr<const Link> link, const std_msgs::ColorRGBA &color){
visualization_msgs::InteractiveMarkerControl control;
// ps.pose = UrdfPose2Pose(link->visual->origin);
visualization_msgs::Marker marker;
//if (use_color) marker.color = color;
marker.type = visualization_msgs::Marker::CYLINDER;
double scale=0.01;
marker.scale.x = scale;
marker.scale.y = scale * 1;
marker.scale.z = scale * 40;
marker.color = color;
//marker.pose = stamped.pose;
//visualization_msgs::InteractiveMarkerControl control;
boost::shared_ptr<Joint> parent_joint = link->parent_joint;
Eigen::Vector3f origin_x(0,0,1);
Eigen::Vector3f dest_x(parent_joint->axis.x, parent_joint->axis.y, parent_joint->axis.z);
Eigen::Quaternionf qua;
qua.setFromTwoVectors(origin_x, dest_x);
marker.pose.orientation.x = qua.x();
marker.pose.orientation.y = qua.y();
marker.pose.orientation.z = qua.z();
marker.pose.orientation.w = qua.w();
control.markers.push_back( marker );
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
control.always_visible = true;
int_marker.controls.push_back(control);
return;
}
geometry_msgs::PoseStamped SnapIt::alignPose(
Eigen::Affine3f& pose, jsk_recognition_utils::ConvexPolygon::Ptr convex)
{
Eigen::Affine3f aligned_pose(pose);
Eigen::Vector3f original_point(pose.translation());
Eigen::Vector3f projected_point;
convex->project(original_point, projected_point);
Eigen::Vector3f normal = convex->getNormal();
Eigen::Vector3f old_normal;
old_normal[0] = pose(0, 2);
old_normal[1] = pose(1, 2);
old_normal[2] = pose(2, 2);
Eigen::Quaternionf rot;
if (normal.dot(old_normal) < 0) {
normal = - normal;
}
rot.setFromTwoVectors(old_normal, normal);
aligned_pose = aligned_pose * rot;
aligned_pose.translation() = projected_point;
Eigen::Affine3d aligned_posed;
convertEigenAffine3(aligned_pose, aligned_posed);
geometry_msgs::PoseStamped ret;
tf::poseEigenToMsg(aligned_posed, ret.pose);
return ret;
}
void makeJointMarker(std::string jointName)
{
boost::shared_ptr<const urdf::Joint> targetJoint = huboModel.getJoint(jointName);
// The marker must be created in the parent frame so you don't get feedback when you move it
visualization_msgs::InteractiveMarker marker;
marker.scale = .125;
marker.name = jointName;
marker.header.frame_id = targetJoint->parent_link_name;
geometry_msgs::Pose controlPose = hubo_motion_ros::toPose( targetJoint->parent_to_joint_origin_transform);
marker.pose = controlPose;
visualization_msgs::InteractiveMarkerControl control;
Eigen::Quaternionf jointAxis;
Eigen::Vector3f axisVector = hubo_motion_ros::toEVector3(targetJoint->axis);
jointAxis.setFromTwoVectors(Eigen::Vector3f::UnitX(), axisVector);
control.orientation.w = jointAxis.w();
control.orientation.x = jointAxis.x();
control.orientation.y = jointAxis.y();
control.orientation.z = jointAxis.z();
control.always_visible = true;
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
marker.controls.push_back(control);
gIntServer->insert(marker);
gIntServer->setCallback(marker.name, &processFeedback);
}
bool CObjTreePlugin::srvGetPlane(srs_env_model::GetPlane::Request &req, srs_env_model::GetPlane::Response &res)
{
const objtree::Object *object = m_octree.object(req.object_id);
//Object hasn't been found
if(!object) return true;
if(object->type() != objtree::Object::PLANE) return true;
const objtree::Plane *plane = (const objtree::Plane*)object;
res.plane.id = req.object_id;
res.plane.pose.position.x = plane->pos().x;
res.plane.pose.position.y = plane->pos().y;
res.plane.pose.position.z = plane->pos().z;
//Quaternion from normal
Eigen::Vector3f normal(plane->normal().x, plane->normal().y, plane->normal().z);
Eigen::Quaternionf q;
q.setFromTwoVectors(upVector, normal.normalized());
res.plane.pose.orientation.x = q.x();
res.plane.pose.orientation.y = q.y();
res.plane.pose.orientation.z = q.z();
res.plane.pose.orientation.w = q.w();
res.plane.scale.x = plane->boundingMax().x-plane->boundingMin().x;
res.plane.scale.y = plane->boundingMax().y-plane->boundingMin().y;
res.plane.scale.z = plane->boundingMax().z-plane->boundingMin().z;
return true;
}
void Plane::project(const Eigen::Affine3f& pose, Eigen::Affine3f& output)
{
Eigen::Vector3f p(pose.translation());
Eigen::Vector3f output_p;
project(p, output_p);
Eigen::Quaternionf rot;
rot.setFromTwoVectors(pose.rotation() * Eigen::Vector3f::UnitZ(),
coordinates().rotation() * Eigen::Vector3f::UnitZ());
output = Eigen::Affine3f::Identity() * Eigen::Translation3f(output_p) * rot;
}
void Plane::initializeCoordinates()
{
Eigen::Quaternionf rot;
rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), normal_);
double c = normal_[2];
double z = 0.0;
// ax + by + cz + d = 0
// z = - d / c (when x = y = 0)
if (c == 0.0) { // its not good
z = 0.0;
}
else {
z = - d_ / c;
}
plane_coordinates_
= Eigen::Affine3f::Identity() * Eigen::Translation3f(0, 0, z) * rot;
}
void UrdfModelMarker::addMoveMarkerControl(visualization_msgs::InteractiveMarker &int_marker, boost::shared_ptr<const Link> link, bool root){
visualization_msgs::InteractiveMarkerControl control;
if(root){
im_helpers::add6DofControl(int_marker,false);
for(int i=0; i<int_marker.controls.size(); i++){
int_marker.controls[i].always_visible = true;
}
}else{
boost::shared_ptr<Joint> parent_joint = link->parent_joint;
Eigen::Vector3f origin_x(1,0,0);
Eigen::Vector3f dest_x(parent_joint->axis.x, parent_joint->axis.y, parent_joint->axis.z);
Eigen::Quaternionf qua;
qua.setFromTwoVectors(origin_x, dest_x);
control.orientation.x = qua.x();
control.orientation.y = qua.y();
control.orientation.z = qua.z();
control.orientation.w = qua.w();
int_marker.scale = 0.5;
switch(parent_joint->type){
case Joint::REVOLUTE:
case Joint::CONTINUOUS:
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(control);
break;
case Joint::PRISMATIC:
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);
break;
default:
break;
}
}
}
void TorusFinder::segment(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
if (!done_initialization_) {
return;
}
boost::mutex::scoped_lock lock(mutex_);
vital_checker_->poke();
pcl::PointCloud<pcl::PointNormal>::Ptr cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*cloud_msg, *cloud);
jsk_recognition_utils::ScopedWallDurationReporter r
= timer_.reporter(pub_latest_time_, pub_average_time_);
if (voxel_grid_sampling_) {
pcl::PointCloud<pcl::PointNormal>::Ptr downsampled_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::VoxelGrid<pcl::PointNormal> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
sor.filter (*downsampled_cloud);
cloud = downsampled_cloud;
}
pcl::SACSegmentation<pcl::PointNormal> seg;
if (use_normal_) {
pcl::SACSegmentationFromNormals<pcl::PointNormal, pcl::PointNormal> seg_normal;
seg_normal.setInputNormals(cloud);
seg = seg_normal;
}
seg.setOptimizeCoefficients (true);
seg.setInputCloud(cloud);
seg.setRadiusLimits(min_radius_, max_radius_);
if (algorithm_ == "RANSAC") {
seg.setMethodType(pcl::SAC_RANSAC);
}
else if (algorithm_ == "LMEDS") {
seg.setMethodType(pcl::SAC_LMEDS);
}
else if (algorithm_ == "MSAC") {
seg.setMethodType(pcl::SAC_MSAC);
}
else if (algorithm_ == "RRANSAC") {
seg.setMethodType(pcl::SAC_RRANSAC);
}
else if (algorithm_ == "RMSAC") {
seg.setMethodType(pcl::SAC_RMSAC);
}
else if (algorithm_ == "MLESAC") {
seg.setMethodType(pcl::SAC_MLESAC);
}
else if (algorithm_ == "PROSAC") {
seg.setMethodType(pcl::SAC_PROSAC);
}
seg.setDistanceThreshold (outlier_threshold_);
seg.setMaxIterations (max_iterations_);
seg.setModelType(pcl::SACMODEL_CIRCLE3D);
if (use_hint_) {
seg.setAxis(hint_axis_);
seg.setEpsAngle(eps_hint_angle_);
}
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
seg.segment(*inliers, *coefficients);
NODELET_INFO("input points: %lu", cloud->points.size());
if (inliers->indices.size() > min_size_) {
// check direction. Torus should direct to origin of pointcloud
// always.
Eigen::Vector3f dir(coefficients->values[4],
coefficients->values[5],
coefficients->values[6]);
if (dir.dot(Eigen::Vector3f::UnitZ()) < 0) {
dir = - dir;
}
Eigen::Affine3f pose = Eigen::Affine3f::Identity();
Eigen::Vector3f pos = Eigen::Vector3f(coefficients->values[0],
coefficients->values[1],
coefficients->values[2]);
Eigen::Quaternionf rot;
rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), dir);
pose = pose * Eigen::Translation3f(pos) * Eigen::AngleAxisf(rot);
PCLIndicesMsg ros_inliers;
ros_inliers.indices = inliers->indices;
ros_inliers.header = cloud_msg->header;
pub_inliers_.publish(ros_inliers);
PCLModelCoefficientMsg ros_coefficients;
ros_coefficients.values = coefficients->values;
ros_coefficients.header = cloud_msg->header;
pub_coefficients_.publish(ros_coefficients);
jsk_recognition_msgs::Torus torus_msg;
torus_msg.header = cloud_msg->header;
tf::poseEigenToMsg(pose, torus_msg.pose);
torus_msg.small_radius = 0.01;
torus_msg.large_radius = coefficients->values[3];
pub_torus_.publish(torus_msg);
jsk_recognition_msgs::TorusArray torus_array_msg;
torus_array_msg.header = cloud_msg->header;
torus_array_msg.toruses.push_back(torus_msg);
pub_torus_array_.publish(torus_array_msg);
// publish pose stamped
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header = torus_msg.header;
pose_stamped.pose = torus_msg.pose;
pub_pose_stamped_.publish(pose_stamped);
pub_torus_array_with_failure_.publish(torus_array_msg);
pub_torus_with_failure_.publish(torus_msg);
}
else {
jsk_recognition_msgs::Torus torus_msg;
torus_msg.header = cloud_msg->header;
torus_msg.failure = true;
pub_torus_with_failure_.publish(torus_msg);
jsk_recognition_msgs::TorusArray torus_array_msg;
torus_array_msg.header = cloud_msg->header;
torus_array_msg.toruses.push_back(torus_msg);
pub_torus_array_with_failure_.publish(torus_array_msg);
NODELET_INFO("failed to find torus");
}
}
void callbackClusteredClouds(const clustered_clouds_msgs::ClusteredCloudsConstPtr& msg)
{
#if PUBLISH_TRANSFORM
static tf::TransformBroadcaster tf_br;
#endif
g_marker_array.markers.clear();
g_marker_id = 0;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<tf::Vector3> hand_positions, arm_directions;
mutex_config.lock();
for(size_t i = 0; i < msg->clouds.size(); i++)
{
bool cloud_with_rgb_data = false;
std::string field_list = pcl::getFieldsList (msg->clouds[i]);
if(field_list.rfind("rgb") != std::string::npos)
{
cloud_with_rgb_data = true;
}
if(cloud_with_rgb_data)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr hand_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr finger_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
tf::Vector3 hand_position, arm_position, arm_direction;
bool found = checkCloud<pcl::PointXYZRGB>(msg->clouds[i],
hand_cloud,
//finger_cloud,
msg->header.frame_id,
hand_position,
arm_position,
arm_direction);
if(found)
{
hand_positions.push_back(hand_position);
arm_directions.push_back(arm_direction);
*cloud_out += *hand_cloud;
}
}
//else
//{
//checkCloud<pcl::PointXYZ>(msg->clouds[i], msg->header.frame_id);
//}
}
mutex_config.unlock();
if(hand_positions.size() > g_hand_trackers.size())
{
std::vector<tf::Vector3> hand_positions_tmp = hand_positions;
cv::Mat result;
for (size_t i = 0; i < g_hand_trackers.size(); i++)
{
result = g_hand_trackers[i].first.lastResult();
tf::Vector3 point1(result.at<float>(0), result.at<float>(1), result.at<float>(2));
int index = closestPoint(point1, hand_positions_tmp);
//remove
hand_positions_tmp.erase(hand_positions_tmp.begin() + index);
}
//add new tracker
for(size_t i = 0; i < hand_positions_tmp.size(); i++)
{
KalmanFilter3d tracker;
cv::Point3f point(hand_positions_tmp[i].x(),
hand_positions_tmp[i].y(),
hand_positions_tmp[i].z());
tracker.initialize(1.0 / 30.0, point, 1e-2, 1e-1, 1e-1);
TrackerWithHistory t;
t.first = tracker;
g_hand_trackers.push_back(t);
}
}
cv::Mat result;
std::vector<tf::Vector3> results;
interaction_msgs::ArmsPtr arms_msg(new interaction_msgs::Arms);
arms_msg->arms.resize(g_hand_trackers.size());
for (size_t i = 0; i < g_hand_trackers.size(); i++)
{
g_hand_trackers[i].first.predict(result);
results.push_back(tf::Vector3(result.at<float>(0), result.at<float>(1), result.at<float>(2)));
arms_msg->arms[i].hand.rotation.w = 1;
}
int index;
cv::Point3f measurement;
tf::Quaternion q_hand;
Eigen::Quaternionf q;
tf::Quaternion q_rotate;
q_rotate.setEuler(0, 0, M_PI);
int last_index = -1;
for(size_t i = 0; i < hand_positions.size(); i++)
{
index = closestPoint(hand_positions[i], results);
if(last_index == index)
continue;
last_index = index;
measurement.x = hand_positions[i].x();
measurement.y = hand_positions[i].y();
measurement.z = hand_positions[i].z();
g_hand_trackers[index].first.update(measurement, result);
results[index] = tf::Vector3(result.at<float>(0), result.at<float>(1), result.at<float>(2));
q.setFromTwoVectors(Eigen::Vector3f(1, 0, 0),
Eigen::Vector3f(arm_directions[i].x(), arm_directions[i].y(), arm_directions[i].z()));
tf::Quaternion q_tf(q.x(), q.y(), q.z(), q.w());
q_hand = q_tf * q_rotate;
arms_msg->arms[index].hand.rotation.x = q_hand.x();
arms_msg->arms[index].hand.rotation.y = q_hand.y();
arms_msg->arms[index].hand.rotation.z = q_hand.z();
arms_msg->arms[index].hand.rotation.w = q_hand.w();
}
for(size_t i = 0; i < results.size(); i++)
{
g_hand_trackers[i].first.updateState();
arms_msg->arms[i].arm_id = g_hand_trackers[i].first.id();
arms_msg->arms[i].hand.translation.x = results[i].x();
arms_msg->arms[i].hand.translation.y = results[i].y();
arms_msg->arms[i].hand.translation.z = results[i].z();
//prepare markers if needed
if(g_marker_array_pub.getNumSubscribers() != 0)
{
geometry_msgs::Point marker_point;
marker_point.x = results[i].x();
marker_point.y = results[i].y();
marker_point.z = results[i].z();
g_hand_trackers[i].second.push_back(marker_point);
if(g_hand_trackers[i].second.size() > HAND_HISTORY_SIZE)
g_hand_trackers[i].second.pop_front();
}
#if PUBLISH_TRANSFORM
tf::Transform hand_tf;
hand_tf.setOrigin(results[i]);
if(index != -1)
{
hand_tf.setRotation(tf::Quaternion(arms_msg->arms[i].hand.rotation.x,
arms_msg->arms[i].hand.rotation.y,
arms_msg->arms[i].hand.rotation.z,
arms_msg->arms[i].hand.rotation.w));
}
else
{
hand_tf.setRotation(tf::Quaternion(0, 0, 0, 1));
}
std::stringstream ss;
ss << "hand" << g_hand_trackers[i].first.id();
tf_br.sendTransform(tf::StampedTransform(hand_tf, ros::Time::now(), msg->header.frame_id, ss.str()));
#endif
}
//remove dead tracker
std::vector<TrackerWithHistory>::iterator it = g_hand_trackers.begin();
while(it != g_hand_trackers.end())
{
if(it->first.getState() == KalmanFilter3d::DIE)
{
ROS_DEBUG("remove tracker %d", it->first.id());
it = g_hand_trackers.erase(it);
}
else
{
it++;
}
}
//prepare markers if needed
if(g_marker_array_pub.getNumSubscribers() != 0)
{
Marker marker;
marker.type = Marker::LINE_STRIP;
marker.lifetime = ros::Duration(0.1);
marker.header.frame_id = msg->header.frame_id;
marker.scale.x = 0.005;
marker.pose.position.x = 0;
marker.pose.position.y = 0;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0;
marker.pose.orientation.y = 0;
marker.pose.orientation.z = 0;
marker.pose.orientation.w = 1;
marker.ns = "point_history";
for (size_t i = 0; i < g_hand_trackers.size(); i++)
{
marker.points.clear();
if (g_hand_trackers[i].second.size() != 0)
{
marker.id = g_marker_id++;
std::list<geometry_msgs::Point>::iterator it;
for (it = g_hand_trackers[i].second.begin(); it != g_hand_trackers[i].second.end(); it++)
{
marker.points.push_back(*it);
}
marker.color.r = 1;
marker.color.g = 0;
marker.color.b = 0;
marker.color.a = 1.0;
g_marker_array.markers.push_back(marker);
}
}
}
if(g_arms_pub.getNumSubscribers() != 0)
{
arms_msg->header.stamp = ros::Time::now();
arms_msg->header.frame_id = msg->header.frame_id;
g_arms_pub.publish(arms_msg);
}
if(g_cloud_pub.getNumSubscribers() != 0)
{
sensor_msgs::PointCloud2 cloud_out_msg;
pcl::toROSMsg(*cloud_out, cloud_out_msg);
cloud_out_msg.header.stamp = ros::Time::now();
cloud_out_msg.header.frame_id = msg->header.frame_id;
g_cloud_pub.publish(cloud_out_msg);
}
if((g_marker_array_pub.getNumSubscribers() != 0) && (!g_marker_array.markers.empty()))
{
g_marker_array_pub.publish(g_marker_array);
}
}
void CObjTreePlugin::showObject(unsigned int id)
{
const objtree::Object *object = m_octree.object(id);
if(!object) return;
char name[64];
snprintf(name, sizeof(name), "imn%u", id);
switch(object->type())
{
case objtree::Object::ALIGNED_BOUNDING_BOX:
{
objtree::BBox *box = (objtree::BBox*)object;
srs_interaction_primitives::AddBoundingBox addBoxSrv;
addBoxSrv.request.frame_id = IM_SERVER_FRAME_ID;
addBoxSrv.request.name = name;
addBoxSrv.request.description = name;
addBoxSrv.request.pose.position.x = box->box().x+box->box().w/2;
addBoxSrv.request.pose.position.y = box->box().y+box->box().h/2;
addBoxSrv.request.pose.position.z = box->box().z+box->box().d/2;
addBoxSrv.request.pose.orientation.x = 0.0f;
addBoxSrv.request.pose.orientation.y = 0.0f;
addBoxSrv.request.pose.orientation.z = 0.0f;
addBoxSrv.request.pose.orientation.w = 1.0f;
addBoxSrv.request.scale.x = box->box().w;
addBoxSrv.request.scale.y = box->box().h;
addBoxSrv.request.scale.z = box->box().d;
addBoxSrv.request.color.r = 1.0;
addBoxSrv.request.color.g = addBoxSrv.request.color.b = 0.0;
addBoxSrv.request.color.a = 1.0;
m_clientAddBoundingBox.call(addBoxSrv);
}
break;
case objtree::Object::GENERAL_BOUNDING_BOX:
{
objtree::GBBox *box = (objtree::GBBox*)object;
srs_interaction_primitives::AddBoundingBox addBoxSrv;
addBoxSrv.request.frame_id = IM_SERVER_FRAME_ID;
addBoxSrv.request.name = name;
addBoxSrv.request.description = name;
addBoxSrv.request.pose.position.x = box->position().x;
addBoxSrv.request.pose.position.y = box->position().y;
addBoxSrv.request.pose.position.z = box->position().z;
addBoxSrv.request.pose.orientation.x = box->orientation().x;
addBoxSrv.request.pose.orientation.y = box->orientation().y;
addBoxSrv.request.pose.orientation.z = box->orientation().z;
addBoxSrv.request.pose.orientation.w = box->orientation().w;
addBoxSrv.request.scale.x = box->scale().x;
addBoxSrv.request.scale.y = box->scale().y;
addBoxSrv.request.scale.z = box->scale().z;
addBoxSrv.request.color.r = 1.0;
addBoxSrv.request.color.g = addBoxSrv.request.color.b = 0.0;
addBoxSrv.request.color.a = 1.0;
m_clientAddBoundingBox.call(addBoxSrv);
}
break;
case objtree::Object::PLANE:
{
objtree::Plane *plane = (objtree::Plane*)object;
srs_interaction_primitives::AddPlane addPlaneSrv;
addPlaneSrv.request.frame_id = IM_SERVER_FRAME_ID;
addPlaneSrv.request.name = name;
addPlaneSrv.request.description = name;
addPlaneSrv.request.pose.position.x = plane->pos().x;
addPlaneSrv.request.pose.position.y = plane->pos().y;
addPlaneSrv.request.pose.position.z = plane->pos().z;
//Quaternion from normal
Eigen::Vector3f normal(plane->normal().x, plane->normal().y, plane->normal().z);
Eigen::Quaternionf q;
q.setFromTwoVectors(upVector, normal.normalized());
addPlaneSrv.request.pose.orientation.x = q.x();
addPlaneSrv.request.pose.orientation.y = q.y();
addPlaneSrv.request.pose.orientation.z = q.z();
addPlaneSrv.request.pose.orientation.w = q.w();
addPlaneSrv.request.scale.x = plane->boundingMax().x-plane->boundingMin().x;
addPlaneSrv.request.scale.y = plane->boundingMax().y-plane->boundingMin().y;
addPlaneSrv.request.scale.z = plane->boundingMax().z-plane->boundingMin().z;
addPlaneSrv.request.color.r = 1.0;
addPlaneSrv.request.color.g = addPlaneSrv.request.color.b = 0.0;
addPlaneSrv.request.color.a = 1.0;
m_clientAddPlane.call(addPlaneSrv);
}
break;
}
}