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; }