void PeriodicThreadImplQNX::makePeriodic()
{
if (m_chid == -1)
{
LOGGING_ERROR_C(Thread, PeriodicThreadImplQNX,
"No timer channel available! Cannot make this thread periodic!" << endl);
return;
}
struct sigevent event;
int coid;
coid = ConnectAttach(0, 0, m_chid, 0, 0);
if (coid == -1)
{
LOGGING_ERROR_C(Thread, PeriodicThreadImplQNX,
"Could not attach to the timer channel! Cannot make this thread periodic!" << endl);
return;
}
SIGEV_PULSE_INIT(&event, coid, SIGEV_PULSE_PRIO_INHERIT, ePT_PULSE_TIMER, 0);
int res = timer_create(CLOCK_REALTIME, &event, &m_timerid);
if (res == -1)
{
LOGGING_ERROR_C(Thread, PeriodicThreadImplQNX,
"Could not create timer! Cannot make this thread periodic!" << endl);
return;
}
m_made_periodic = true;
setPeriod(m_period);
}
void Robot::setConfiguration(const std::map<std::string, float> &joint_values)
{
// Reset all calculation flags.
for (M_NameToLink::const_iterator link=links_.begin(); link != links_.end(); link++)
{
link->second->resetPoseCalculated();
}
for (M_NameToJoint::const_iterator joint=joints_.begin(); joint != joints_.end(); joint++)
{
joint->second->resetPoseCalculated();
}
for (std::map<std::string, float>::const_iterator joint=joint_values.begin(); joint != joint_values.end(); joint++)
{
RobotJoint* rob_joint = getJoint(joint->first);
if(rob_joint)
{
rob_joint->setJointValue(joint->second);
}else{
LOGGING_ERROR_C(RobotLog, Robot,
"Joint " << joint->first << " not found and not updated." << endl);
}
}
}
KDL::Frame RobotLink::getPose()
{
KDL::Frame ret;
KDL::Frame parent_pose;
KDL::Frame local_pose;
RobotJoint* parent_joint;
if(!pose_calculated_)
{
// if this is the root link, the parent pose is the robot pose
if(this == robot_->getRootLink())
{
pose_property_ = robot_->getPose();
}else{
parent_joint = robot_->getJoint(parent_joint_name_);
if(parent_joint)
{
parent_pose = parent_joint->getPose();
}else{
LOGGING_ERROR_C(RobotLog, RobotLink,
"RobotLink::getPose() could not find a parent joint called ["
<< parent_joint_name_ << "]!" << endl);
return ret;
}
// we now have the parent pose, around which we rotate the current segment:
std::map<std::string,KDL::TreeElement>::const_iterator local_segment = robot_->getTree().getSegment(name_);
if(local_segment != robot_->getTree().getSegments().end())
{
local_pose = local_segment->second.segment.pose(parent_joint->getJointValue());
visual_node_->setPose(parent_pose);
}else{
LOGGING_ERROR_C(RobotLog, RobotLink,
"RobotLink::getPose() could not find local KDL segment named [" << name_ << "]!" << endl);
return ret;
}
pose_property_ = parent_pose * local_pose;
}
pose_calculated_ = true;
}
return pose_property_;
}
void CanOpenController::addNode(const uint8_t node_id, const std::string& group_name)
{
std::string sanitized_identifier = sanitizeString(group_name);
std::map<std::string, DS301Group::Ptr>::iterator group_it;
group_it = m_groups.find(sanitized_identifier);
if (m_nodes.find(node_id) == m_nodes.end())
{
if (group_it != m_groups.end())
{
DS301Node::Ptr new_node;
// TODO: Maybe this can be done prettier with templates, however for now this works
DS402Group::Ptr ds402_ptr;
if (ds402_ptr = boost::dynamic_pointer_cast<DS402Group>(group_it->second))
{
new_node = ds402_ptr->addNode<NodeT>(node_id, m_can_device, m_heartbeat_monitor);
}
else
{
new_node = group_it->second->addNode<NodeT>(node_id, m_can_device, m_heartbeat_monitor);
}
#ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
new_node->registerWSBroadcaster(m_ws_broadcaster);
#endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_
m_nodes.insert (std::pair<uint8_t, DS301Node::Ptr>(node_id, new_node));
}
else
{
LOGGING_ERROR_C(CanOpen, CanOpenController, "No group with the given index " << sanitized_identifier
<< " exists. New node not added!" << endl);
}
}
else
{
LOGGING_ERROR_C(CanOpen, CanOpenController, "Node with CANOPEN ID " << node_id
<< " already exists. Not adding new node." << endl);
}
}
RobotJoint* Robot::getJoint( const std::string& name )
{
M_NameToJoint::iterator it = joints_.find( name );
if ( it == joints_.end() )
{
LOGGING_ERROR_C(RobotLog, Robot,
"Joint " << name << " does not exist!" << endl);
return NULL;
}
return it->second;
}
RobotLink* Robot::getLink( const std::string& name )
{
M_NameToLink::iterator it = links_.find( name );
if ( it == links_.end() )
{
LOGGING_ERROR_C(RobotLog, Robot,
"Link " << name << " does not exist!" << endl);
return NULL;
}
return it->second;
}
PeriodicThreadImplQNX::PeriodicThreadImplQNX(const icl_core::TimeSpan& period)
: m_period(period),
m_made_periodic(false),
m_chid(-1)
{
m_chid = ChannelCreate(0);
if (m_chid == -1)
{
LOGGING_ERROR_C(Thread, PeriodicThreadImplQNX,
"Could not create timer channel." << endl);
}
}
void CanOpenController::addGroup(const std::string& identifier)
{
std::string sanitized_identifier = sanitizeString(identifier);
if (m_groups.find(sanitized_identifier) == m_groups.end())
{
DS301Group::Ptr group(new GroupT(sanitized_identifier));
#ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
group->registerWSBroadcaster(m_ws_broadcaster);
#endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_
m_groups[sanitized_identifier] = group;
}
else
{
LOGGING_ERROR_C(CanOpen, CanOpenController, "Group with the given identifier " << sanitized_identifier
<< " already exists. Not adding new group." << endl);
}
}
boost::shared_ptr<GroupT> CanOpenController::getGroup (const std::string& index)
{
std::string sanitized_index = sanitizeString(index);
boost::shared_ptr<GroupT> group;
if (m_groups.find(sanitized_index) != m_groups.end())
{
group = boost::dynamic_pointer_cast<GroupT>(m_groups[sanitized_index]);
if (!group)
{
LOGGING_ERROR_C(CanOpen, CanOpenController, "Cannot cast group to requested type. Returning null pointer." << endl);
}
}
else
{
std::stringstream ss;
ss << "No group with the given index " << sanitized_index
<< " exists. Returning null pointer.";
throw NotFoundException(ss.str());
}
return group;
}
void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision )
{
// clear out any data (properties, shapes, etc) from a previously loaded robot.
clear();
// the root link is discovered below. Set to NULL until found.
root_link_ = NULL;
// Create properties for each link.
{
typedef std::map<std::string, boost::shared_ptr<urdf::Link> > M_NameToUrdfLink;
M_NameToUrdfLink::const_iterator link_it = urdf.links_.begin();
M_NameToUrdfLink::const_iterator link_end = urdf.links_.end();
for( ; link_it != link_end; ++link_it )
{
const boost::shared_ptr<const urdf::Link>& urdf_link = link_it->second;
std::string parent_joint_name;
if (urdf_link != urdf.getRoot() && urdf_link->parent_joint)
{
parent_joint_name = urdf_link->parent_joint->name;
}
RobotLink* link = new RobotLink(this, urdf_link, parent_joint_name,
visual, collision, link_pointclouds_);
if (urdf_link == urdf.getRoot())
{
root_link_ = link;
}
links_[urdf_link->name] = link;
}
}
// Create properties for each joint.
{
typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > M_NameToUrdfJoint;
M_NameToUrdfJoint::const_iterator joint_it = urdf.joints_.begin();
M_NameToUrdfJoint::const_iterator joint_end = urdf.joints_.end();
for( ; joint_it != joint_end; ++joint_it )
{
const boost::shared_ptr<const urdf::Joint>& urdf_joint = joint_it->second;
RobotJoint* joint = new RobotJoint(this, urdf_joint);
joints_[urdf_joint->name] = joint;
}
}
link_pointclouds_.syncToDevice(); // after all links have been created, we sync them to the GPU
// finally create a KDL representation of the kinematic tree:
if (!kdl_parser::treeFromUrdfModel(urdf, tree)){
LOGGING_ERROR_C(RobotLog, Robot,
"Failed to extract kdl tree from xml robot description!" << endl);
exit(-1);
}
LOGGING_INFO_C(RobotLog, Robot,
"Constructed KDL tree has " << tree.getNrOfJoints() << " Joints and "
<< tree.getNrOfSegments() << " segments." << endl);
}
void RobotLink::createEntityForGeometryElement(const urdf::LinkConstPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, node*& entity)
{
offset_node = new node();
gpu_voxels::Vector3f scale;
KDL::Vector offset_pos = KDL::Vector(origin.position.x, origin.position.y, origin.position.z);
KDL::Rotation offset_rot = KDL::Rotation::Quaternion(origin.rotation.x, origin.rotation.y, origin.rotation.z, origin.rotation.w);
KDL::Frame offset_pose = KDL::Frame(offset_rot, offset_pos);
switch (geom.type)
{
case urdf::Geometry::SPHERE:
{
LOGGING_WARNING_C(RobotLog, RobotLink,
"Link " << std::string(link->name) <<
" has SPHERE geometry, which is not supported!" << endl);
break;
}
case urdf::Geometry::BOX:
{
LOGGING_WARNING_C(RobotLog, RobotLink,
"Link " << link->name <<
" has BOX geometry, which is not supported!" << endl);
break;
}
case urdf::Geometry::CYLINDER:
{
LOGGING_WARNING_C(RobotLog, RobotLink,
"Link " << link->name <<
" has CYLINDER geometry, which is not supported!" << endl);
break;
}
case urdf::Geometry::MESH:
{
const urdf::Mesh& mesh = static_cast<const urdf::Mesh&>(geom);
if ( mesh.filename.empty() )
return;
scale = gpu_voxels::Vector3f(mesh.scale.x, mesh.scale.y, mesh.scale.z);
fs::path p(mesh.filename);
fs::path pc_file = fs::path(p.stem().string() + std::string(".binvox"));
std::vector<Vector3f> link_cloud;
LOGGING_DEBUG_C(RobotLog, RobotLink, "Loading pointcloud of link " << pc_file.string() << endl);
if(!file_handling::PointcloudFileHandler::Instance()->loadPointCloud(
pc_file.string(), use_model_path_, link_cloud, false, Vector3f(), 1.0))
{
LOGGING_ERROR_C(RobotLog, RobotLink,
"Could not read file [" << pc_file.string() <<
"]. Adding single point instead..." << endl);
link_cloud.push_back(Vector3f());
}
link_pointclouds_.addCloud(link_cloud, false, name_);
entity->setHasData(true);
break;
}
default:
LOGGING_WARNING_C(RobotLog, RobotLink,
"Link " << link->name <<
" has unsupported geometry type " << geom.type << "." << endl);
break;
}
if ( entity )
{
offset_node->setScale(scale);
offset_node->setPose(offset_pose);
}
}
/*!
* \brief loadPointCloud loads a PCD file and returns the points in a vector.
* \param path Filename
* \param points points are written into this vector
* \param shift_to_zero If true, the pointcloud is shifted, so its minimum coordinates lie at zero
* \param offset_XYZ Additional transformation offset
* \return true if succeeded, false otherwise
*/
bool PointcloudFileHandler::loadPointCloud(const std::string _path, const bool use_model_path, std::vector<Vector3f> &points, const bool shift_to_zero,
const Vector3f &offset_XYZ, const float scaling)
{
std::string path;
// if param is true, prepend the environment variable GPU_VOXELS_MODEL_PATH
path = (getGpuVoxelsPath(use_model_path) / boost::filesystem::path(_path)).string();
LOGGING_DEBUG_C(
Gpu_voxels_helpers,
GpuVoxelsMap,
"Loading Pointcloud file " << path << " ..." << endl);
// is the file a simple xyz file?
std::size_t found = path.find(std::string("xyz"));
if (found!=std::string::npos)
{
if (!xyz_reader->readPointCloud(path, points))
{
return false;
}
}else{
// is the file a simple pcl pcd file?
std::size_t found = path.find(std::string("pcd"));
if (found!=std::string::npos)
{
#ifdef _BUILD_GVL_WITH_PCL_SUPPORT_
if (!pcd_reader->readPointCloud(path, points))
{
return false;
}
#else
LOGGING_ERROR_C(
Gpu_voxels_helpers,
GpuVoxelsMap,
"Your GPU-Voxels was built without PCD support!" << endl);
return false;
#endif
}else{
// is the file a binvox file?
std::size_t found = path.find(std::string("binvox"));
if (found!=std::string::npos)
{
if (!binvox_reader->readPointCloud(path, points))
{
return false;
}
}else{
LOGGING_ERROR_C(
Gpu_voxels_helpers,
GpuVoxelsMap,
path << " has no known file format." << endl);
return false;
}
}
}
if (shift_to_zero)
{
shiftPointCloudToZero(points);
}
for (size_t i = 0; i < points.size(); i++)
{
points[i].x = (scaling * points[i].x) + offset_XYZ.x;
points[i].y = (scaling * points[i].y) + offset_XYZ.y;
points[i].z = (scaling * points[i].z) + offset_XYZ.z;
}
return true;
}
