planning_models::KinematicModel::LinkModel* planning_models::KinematicModel::constructLinkModel(const urdf::Link *urdf_link)
{
ROS_ASSERT(urdf_link);
LinkModel *result = new LinkModel(this);
result->name_ = urdf_link->name;
if(urdf_link->collision && urdf_link->collision->geometry) {
result->collision_origin_transform_ = urdfPose2TFTransform(urdf_link->collision->origin);
result->shape_ = constructShape(urdf_link->collision->geometry.get());
} else if(urdf_link->visual && urdf_link->visual->geometry){
result->collision_origin_transform_ = urdfPose2TFTransform(urdf_link->visual->origin);
result->shape_ = constructShape(urdf_link->visual->geometry.get());
} else {
result->collision_origin_transform_.setIdentity();
result->shape_ = NULL;
}
if (urdf_link->parent_joint.get()) {
result->joint_origin_transform_ = urdfPose2TFTransform(urdf_link->parent_joint->parent_to_joint_origin_transform);
ROS_DEBUG_STREAM("Setting joint origin for " << result->name_ << " to "
<< result->joint_origin_transform_.getOrigin().x() << " "
<< result->joint_origin_transform_.getOrigin().y() << " "
<< result->joint_origin_transform_.getOrigin().z());
} else {
ROS_DEBUG_STREAM("Setting joint origin to identity for " << result->name_);
result->joint_origin_transform_.setIdentity();
}
return result;
}
bool robot_self_filter_color::SelfMask::configure(const std::vector<LinkInfo> &links)
{
// in case configure was called before, we free the memory
freeMemory();
sensor_pos_.setValue(0, 0, 0);
std::string content;
boost::shared_ptr<urdf::Model> urdfModel;
if (nh_.getParam("robot_description", content))
{
urdfModel = boost::shared_ptr<urdf::Model>(new urdf::Model());
if (!urdfModel->initString(content))
{
ROS_ERROR("Unable to parse URDF description!");
return false;
}
}
else
{
ROS_ERROR("Robot model not found! Did you remap 'robot_description'?");
return false;
}
std::stringstream missing;
// from the geometric model, find the shape of each link of interest
// and create a body from it, one that knows about poses and can
// check for point inclusion
for (unsigned int i = 0 ; i < links.size() ; ++i)
{
const urdf::Link *link = urdfModel->getLink(links[i].name).get();
if (!link)
{
missing << " " << links[i].name;
continue;
}
if (!(link->collision && link->collision->geometry))
{
ROS_WARN("No collision geometry specified for link '%s'", links[i].name.c_str());
continue;
}
shapes::Shape *shape = constructShape(link->collision->geometry.get());
if (!shape)
{
ROS_ERROR("Unable to construct collision shape for link '%s'", links[i].name.c_str());
continue;
}
SeeLink sl;
sl.body = bodies::createBodyFromShape(shape);
if (sl.body)
{
sl.name = links[i].name;
// collision models may have an offset, in addition to what TF gives
// so we keep it around
sl.constTransf = urdfPose2TFTransform(link->collision->origin);
sl.body->setScale(links[i].scale);
sl.body->setPadding(links[i].padding);
ROS_DEBUG_STREAM("Self see link name " << links[i].name << " padding " << links[i].padding);
sl.volume = sl.body->computeVolume();
sl.unscaledBody = bodies::createBodyFromShape(shape);
bodies_.push_back(sl);
}
else
ROS_WARN("Unable to create point inclusion body for link '%s'", links[i].name.c_str());
delete shape;
}
if (missing.str().size() > 0)
ROS_WARN("Some links were included for self mask but they do not exist in the model:%s", missing.str().c_str());
if (bodies_.empty())
ROS_WARN("No robot links will be checked for self mask");
// put larger volume bodies first -- higher chances of containing a point
std::sort(bodies_.begin(), bodies_.end(), SortBodies());
bspheres_.resize(bodies_.size());
bspheresRadius2_.resize(bodies_.size());
for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
ROS_DEBUG("Self mask includes link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].volume);
//ROS_INFO("Self filter using %f padding and %f scaling", padd, scale);
return true;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "addURDF");
//const std::string frame_id = "/base_footprint";
const std::string frame_id = "/map";
ros::NodeHandle nh;
ros::Publisher object_in_map_pub_;
object_in_map_pub_ = nh.advertise<arm_navigation_msgs::CollisionObject>("collision_object", 20);
std::string parameter_name = "world_description";
std::string model_name = "urdf_world_model";
while(!nh.hasParam(parameter_name))
{
ROS_WARN("waiting for parameter \"world_description\"... ");
ros::Duration(0.5).sleep();
}
urdf::Model model;
if (!model.initParam(parameter_name))
{
ROS_ERROR("Failed to parse %s from parameter server", parameter_name.c_str());
return -1;
}
ROS_INFO("Successfully parsed urdf file");
std::vector< boost::shared_ptr< urdf::Link > > URDF_links;
model.getLinks(URDF_links);
std::map< std::string, boost::shared_ptr< urdf::Joint > > URDF_joints = model.joints_;
std::map< std::string, boost::shared_ptr< urdf::Joint > >::iterator joints_it;
//tranfo between links is in joints->origin!
//access to Joint-Information
for(joints_it=URDF_joints.begin() ; joints_it != URDF_joints.end(); joints_it++)
{
ROS_INFO("Joint name: %s", (*joints_it).first.c_str());
ROS_INFO("\t origin: %f,%f,%f", (*joints_it).second->parent_to_joint_origin_transform.position.x, (*joints_it).second->parent_to_joint_origin_transform.position.y, (*joints_it).second->parent_to_joint_origin_transform.position.z);
}
ros::service::waitForService("/gazebo/get_model_state");
ros::service::waitForService("/cob3_environment_server/get_state_validity"); //just to make sure that the environment_server is there!
//access to tranformation /world to /root_link (table_top)
ros::ServiceClient client = nh.serviceClient<gazebo::GetModelState>("/gazebo/get_model_state");
gazebo::GetModelState srv;
srv.request.model_name = model_name;
if (client.call(srv))
{
ROS_INFO("URDFPose (x,y,z): (%f,%f,%f)", srv.response.pose.position.x, srv.response.pose.position.y, srv.response.pose.position.z);
}
else
{
ROS_ERROR("Failed to call service get_model_state");
ros::shutdown();
}
arm_navigation_msgs::CollisionObject collision_object;
collision_object.id = model_name + "_object";
collision_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
//collision_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE;
collision_object.header.frame_id = frame_id;
collision_object.header.stamp = ros::Time::now();
collision_object.shapes.resize(URDF_links.size());
collision_object.poses.resize(URDF_links.size());
joints_it=URDF_joints.begin();
for(unsigned int i=0; i<URDF_links.size(); i++)
{
urdf::Link current_link = *URDF_links[i];
ROS_INFO("Current Link: %s", current_link.name.c_str());
if(current_link.name == "base_link")
{
ROS_INFO("Dealing with base_link...");
continue;
}
boost::shared_ptr< urdf::Joint > current_parent_joint = current_link.parent_joint;
//ROS_INFO("Current Parent Joint: %s", current_parent_joint->name.c_str());
//fill CollisionObject for each link
shapes::Shape *current_shape;
current_shape = constructShape(current_link.collision->geometry.get());
ROS_INFO("shape.type: %d", current_shape->type);
//ROS_INFO("Position (x,y,z): (%f,%f,%f)", current_link.collision->origin.position.x, current_link.collision->origin.position.y, current_link.collision->origin.position.z);
tf::Pose pose;
tf::Transform world2dummy;
tf::Transform dummy2link;
world2dummy = tf::Transform(tf::Quaternion(srv.response.pose.orientation.x, srv.response.pose.orientation.y, srv.response.pose.orientation.z, srv.response.pose.orientation.w), tf::Vector3(srv.response.pose.position.x, srv.response.pose.position.y, srv.response.pose.position.z));
dummy2link = tf::Transform(tf::Quaternion(current_parent_joint->parent_to_joint_origin_transform.rotation.x,
current_parent_joint->parent_to_joint_origin_transform.rotation.y,
current_parent_joint->parent_to_joint_origin_transform.rotation.z,
current_parent_joint->parent_to_joint_origin_transform.rotation.w),
tf::Vector3(current_parent_joint->parent_to_joint_origin_transform.position.x,
current_parent_joint->parent_to_joint_origin_transform.position.y,
current_parent_joint->parent_to_joint_origin_transform.position.z));
tf::Pose pose2;
pose2.mult(world2dummy, dummy2link);
tf::Stamped<tf::Pose> stamped_pose_in;
stamped_pose_in.stamp_ = ros::Time::now();
stamped_pose_in.frame_id_ = frame_id;
stamped_pose_in.setData(pose2);
//// TODO: fill in collision_object
arm_navigation_msgs::Shape msg_shape;
planning_environment::constructObjectMsg(current_shape, msg_shape);
geometry_msgs::PoseStamped msg_pose_stamped;
//tf::poseStampedTFToMsg (transformed_pose, msg_pose_stamped);
tf::poseStampedTFToMsg (stamped_pose_in, msg_pose_stamped);
collision_object.shapes[i] = msg_shape;
collision_object.poses[i] = msg_pose_stamped.pose;
object_in_map_pub_.publish(collision_object);
ROS_INFO("Should have published");
}
ros::shutdown();
}
