Mesh* createMeshFromAsset(const aiScene* scene, const Eigen::Vector3d &scale, const std::string &resource_name) { if (!scene->HasMeshes()) { ROS_WARN_STREAM("Assimp reports scene in " << resource_name << " has no meshes"); return NULL; } EigenSTL::vector_Vector3d vertices; std::vector<unsigned int> triangles; extractMeshData(scene, scene->mRootNode, aiMatrix4x4(), scale, vertices, triangles); if (vertices.empty()) { ROS_WARN_STREAM("There are no vertices in the scene " << resource_name); return NULL; } if (triangles.empty()) { ROS_WARN_STREAM("There are no triangles in the scene " << resource_name); return NULL; } return createMeshFromVertices(vertices, triangles); }
collision_space::EnvironmentModel::AllowedCollisionMatrix planning_environment::convertFromACMMsgToACM(const arm_navigation_msgs::AllowedCollisionMatrix& matrix) { std::map<std::string, unsigned int> indices; std::vector<std::vector<bool> > vecs; unsigned int ns = matrix.link_names.size(); if(matrix.entries.size() != ns) { ROS_WARN_STREAM("Matrix messed up"); } vecs.resize(ns); for(unsigned int i = 0; i < std::min(ns, (unsigned int) matrix.entries.size()); i++) { indices[matrix.link_names[i]] = i; if(matrix.entries[i].enabled.size() != ns) { ROS_WARN_STREAM("Matrix messed up"); } vecs[i].resize(ns); for(unsigned int j = 0; j < std::min(ns, (unsigned int) matrix.entries[i].enabled.size()); j++) { vecs[i][j] = matrix.entries[i].enabled[j]; } } collision_space::EnvironmentModel::AllowedCollisionMatrix acm(vecs,indices); return acm; }
void CSrf10Controller::timerCallback(const ros::TimerEvent& e) { ros::Time now=ros::Time::now(); std::map<uint8_t,unsigned short> updatedDistances; int code=device_p_->getDistanceSensors(updatedDistances); if (code<0) ROS_ERROR("Unable to get srf10 sensor distances from the base control board"); else { std::map< uint8_t,CDistanceSensor * >::iterator it; for (it=srf10Sensors_.begin();it!=srf10Sensors_.end();it++) { if (updatedDistances.count((*it).first)>0) { ROS_DEBUG_STREAM("Obtained distance " << updatedDistances[(*it).first] << " for srf10 sensor " << (*it).second->getName() << " from the base control board"); //do we publish al lvalues or only when we see something ? (0.0 values means no obstacle) bool publish_if_obstacle; nh.getParam("controllers/"+getName()+"/sensors/front/"+(*it).second->getName()+"/publish_if_obstacle", publish_if_obstacle); if( publish_if_obstacle){ if( updatedDistances[(*it).first]>0) (*it).second->publish((float)updatedDistances[(*it).first],now); } else{ (*it).second->publish((float)updatedDistances[(*it).first],now); } } else ROS_WARN_STREAM("Could not obtain distance of srf10 sensor " << (int)((*it).first) << " from base control board"); } } std::vector<unsigned int> adcReads; code=device_p_->getAdcReads(adcSensorsAddresses_,adcReads); if (code<0) ROS_ERROR("Unable to get adc sensor reads from the base control board"); else { if(adcReads.size()!=adcSensorsAddresses_.size()) ROS_ERROR("The asked addreses and the returned reads for the adc sensors do not match"); else { for (int i=0;i<adcSensorsAddresses_.size();i++) { ROS_DEBUG_STREAM("Obtained distance " << adcReads[i] << " for adc sensor " << adcSensors_[adcSensorsAddresses_[i]]->getName() << " from the base control board"); adcSensors_[adcSensorsAddresses_[i]]->publish(adcReads[i],now); } } } }
void planning_models::KinematicState::JointStateGroup::getKinematicStateValues(std::map<std::string,double>& joint_state_values) const { joint_state_values.clear(); for(unsigned int i = 0; i < joint_state_vector_.size(); i++) { unsigned int dim = joint_state_vector_[i]->getDimension(); if(dim != 0) { for(unsigned int j = 0; j < joint_state_vector_[i]->getJointStateValues().size(); j++) { joint_state_values[joint_state_vector_[i]->getJointStateNameOrder()[j]] = joint_state_vector_[i]->getJointStateValues()[j]; } } } if(joint_state_values.size() != dimension_) { ROS_WARN_STREAM("Some problems with group map dimension values " << joint_state_values.size() << " dimension is " << dimension_); } }
void LagrangeAllocator::setWeights(const Eigen::MatrixXd &W_new) { bool isCorrectDimensions = ( W_new.rows() == r && W_new.cols() == r ); if (!isCorrectDimensions) { ROS_WARN_STREAM("Attempt to set weight matrix in LagrangeAllocator with wrong dimensions " << W_new.rows() << "*" << W_new.cols() << "."); return; } W = W_new; // New weights require recomputing the generalized inverse of the thrust config matrix computeGeneralizedInverse(); }
void planning_models::KinematicState::getKinematicStateValues(std::vector<double>& joint_state_values) const { joint_state_values.clear(); for(unsigned int i = 0; i < joint_state_vector_.size(); i++) { unsigned int dim = joint_state_vector_[i]->getDimension(); if(dim != 0) { for(unsigned int j = 0; j < joint_state_vector_[i]->getJointStateValues().size(); j++) { joint_state_values.push_back(joint_state_vector_[i]->getJointStateValues()[j]); } } } if(joint_state_values.size() != dimension_) { ROS_WARN_STREAM("Some problems with state vector dimension values " << joint_state_values.size() << " dimension is " << dimension_); } }
void MotionPlanningFrame::pathConstraintsIndexChanged(int index) { if (move_group_) { if (index > 0) { std::string c = ui_->path_constraints_combo_box->itemText(index).toStdString(); if (!move_group_->setPathConstraints(c)) ROS_WARN_STREAM("Unable to set the path constraints: " << c); } else move_group_->clearPathConstraints(); } }
double leatherman::getColladaFileScale(std::string resource) { static std::map<std::string, float> rescale_cache; // Try to read unit to meter conversion ratio from mesh. Only valid in Collada XML formats. TiXmlDocument xmlDoc; float unit_scale(1.0); resource_retriever::Retriever retriever; resource_retriever::MemoryResource res; try { res = retriever.get(resource); } catch (resource_retriever::Exception& e) { ROS_ERROR("%s", e.what()); return unit_scale; } if (res.size == 0) { return unit_scale; } // Use the resource retriever to get the data. const char * data = reinterpret_cast<const char * > (res.data.get()); xmlDoc.Parse(data); // Find the appropriate element if it exists if(!xmlDoc.Error()) { TiXmlElement * colladaXml = xmlDoc.FirstChildElement("COLLADA"); if(colladaXml) { TiXmlElement *assetXml = colladaXml->FirstChildElement("asset"); if(assetXml) { TiXmlElement *unitXml = assetXml->FirstChildElement("unit"); if (unitXml && unitXml->Attribute("meter")) { // Failing to convert leaves unit_scale as the default. if(unitXml->QueryFloatAttribute("meter", &unit_scale) != 0) ROS_WARN_STREAM("getMeshUnitRescale::Failed to convert unit element meter attribute to determine scaling. unit element: " << *unitXml); } } } } return unit_scale; }
void Channel::cmdCallback(const roboteq_msgs::Command& command) { // Reset command timeout. timeout_timer_.stop(); timeout_timer_.start(); ROS_WARN_STREAM("Got command " << command.mode << " with " << command.setpoint); // Update mode of motor driver. We send this on each command for redundancy against a // lost message, and the MBS script keeps track of changes and updates the control // constants accordingly. controller_->command << "VAR" << channel_num_ << static_cast<int>(command.mode) << controller_->send; if (command.mode == roboteq_msgs::Command::MODE_VELOCITY) { // Get a -1000 .. 1000 command as a proportion of the maximum RPM. int roboteq_velocity = to_rpm(command.setpoint) / max_rpm_ * 1000.0; ROS_WARN_STREAM("Commanding " << roboteq_velocity << " velocity to motor driver."); // Write mode and command to the motor driver. controller_->command << "G" << channel_num_ << roboteq_velocity << controller_->send; } else if (command.mode == roboteq_msgs::Command::MODE_POSITION) { // Convert the commanded position in rads to encoder ticks. int roboteq_position = to_encoder_ticks(command.setpoint); ROS_DEBUG_STREAM("Commanding " << roboteq_position << " position to motor driver."); // Write command to the motor driver. controller_->command << "P" << channel_num_ << roboteq_position << controller_->send; } else { ROS_WARN_STREAM("Command received with unknown mode number, dropping."); } controller_->flush(); last_mode_ = command.mode; }
bool VectorVisualization::visualize(const grid_map::GridMap& map) { if (!isActive()) return true; for (const auto& type : types_) { if (!map.exists(type)) { ROS_WARN_STREAM( "VectorVisualization::visualize: No grid map layer with name '" << type << "' found."); return false; } } // Set marker info. marker_.header.frame_id = map.getFrameId(); marker_.header.stamp.fromNSec(map.getTimestamp()); // Clear points. marker_.points.clear(); marker_.colors.clear(); for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { if (!map.isValid(*iterator, positionLayer_) || !map.isValid(*iterator, types_)) continue; geometry_msgs::Vector3 vector; vector.x = map.at(types_[0], *iterator); // FIXME(cfo): segfaults when types is not set vector.y = map.at(types_[1], *iterator); vector.z = map.at(types_[2], *iterator); Eigen::Vector3d position; map.getPosition3(positionLayer_, *iterator, position); geometry_msgs::Point startPoint; startPoint.x = position.x(); startPoint.y = position.y(); startPoint.z = position.z(); marker_.points.push_back(startPoint); geometry_msgs::Point endPoint; endPoint.x = startPoint.x + scale_ * vector.x; endPoint.y = startPoint.y + scale_ * vector.y; endPoint.z = startPoint.z + scale_ * vector.z; marker_.points.push_back(endPoint); marker_.colors.push_back(color_); // Each vertex needs a color. marker_.colors.push_back(color_); } publisher_.publish(marker_); return true; }
bool constraint_samplers::JointConstraintSampler::setup(const std::vector<kinematic_constraints::JointConstraint> &jc) { if (!jmg_) { ROS_FATAL("NULL group specified for constraint sampler"); return false; } // find and keep the constraints that operate on the group we sample // also keep bounds for joints as convenient const std::map<std::string, unsigned int> &vim = jmg_->getJointVariablesIndexMap(); std::set<const planning_models::KinematicModel::JointModel*> bounded; for (std::size_t i = 0 ; i < jc.size() ; ++i) { if (!jc[i].enabled()) continue; const planning_models::KinematicModel::JointModel *jm = jc[i].getJointModel(); if (!jmg_->hasJointModel(jm->getName())) continue; std::pair<double, double> bounds; jm->getVariableBounds(jc[i].getJointVariableName(), bounds); bounds.first = std::max(bounds.first, jc[i].getDesiredJointPosition() - jc[i].getJointToleranceBelow()); bounds.second = std::min(bounds.second, jc[i].getDesiredJointPosition() + jc[i].getJointToleranceAbove()); if (bounds.first > bounds.second) { std::stringstream cs; jc[i].print(cs); ROS_WARN_STREAM("The constraints for joint '" << jm->getName() << "' are such that there are no possible values for this joint. Ignoring constraint: \n" << cs.str()); continue; } if (jm->getVariableCount() == 1) bounded.insert(jm); bounds_.push_back(bounds); index_.push_back(vim.find(jc[i].getJointVariableName())->second); } // get a separate list of joints that are not bounded; we will sample these randomly const std::vector<const planning_models::KinematicModel::JointModel*> &joints = jmg_->getJointModels(); for (std::size_t i = 0 ; i < joints.size() ; ++i) if (bounded.find(joints[i]) == bounded.end()) { unbounded_.push_back(joints[i]); uindex_.push_back(vim.find(joints[i]->getName())->second); } values_.resize(jmg_->getVariableCount()); return true; }
/** Open the camera device. * * @param newconfig configuration parameters * @return true, if successful * * if successful: * state_ is Driver::OPENED * camera_name_ set to camera_name string */ bool openCamera(Config &newconfig) { bool success = true; try { ROS_INFO("opening uvc_cam at %dx%d, %f fps", newconfig.width, newconfig.height, newconfig.frame_rate); uvc_cam::Cam::mode_t mode = uvc_cam::Cam::MODE_RGB; switch (newconfig.format_mode) { case 1: mode = uvc_cam::Cam::MODE_RGB; case 2: mode = uvc_cam::Cam::MODE_YUYV; case 3: mode = uvc_cam::Cam::MODE_MJPG; default: mode = uvc_cam::Cam::MODE_RGB; } cam_ = new uvc_cam::Cam(newconfig.device.c_str(), mode, newconfig.width, newconfig.height, newconfig.frame_rate); if (camera_name_ != camera_name_) { camera_name_ = camera_name_; if (!cinfo_.setCameraName(camera_name_)) { // GUID is 16 hex digits, which should be valid. // If not, use it for log messages anyway. ROS_WARN_STREAM("[" << camera_name_ << "] name not valid" << " for camera_info_manger"); } } // ROS_INFO_STREAM("[" << camera_name_ // << "] opened: " << newconfig.video_mode << ", " // << newconfig.frame_rate << " fps, " // << newconfig.iso_speed << " Mb/s"); state_ = Driver::OPENED; calibration_matches_ = true; // cam_->display_formats_supported(); } catch (uvc_cam::Exception& e) { ROS_FATAL_STREAM("[" << camera_name_ << "] exception opening device: " << e.what()); success = false; } return success; }
/** * @brief If not already maxxed, increment the angular velocities.. */ void KeyOp::incrementAngularVelocity() { if (power_status_) { if (cmd_->angular.z <= angular_vel_max_) { cmd_->angular.z += angular_vel_step_; } ROS_INFO_STREAM("KeyOp: angular velocity incremented [" << cmd_->linear.x << "|" << cmd_->angular.z << "]"); } else { ROS_WARN_STREAM("KeyOp: motors are not yet powered up."); } }
void PredictionModel::new_measurement(double x, double y, double z) { if( (x != x) | (y != y) | (z != z) ) { ROS_WARN_STREAM("We received NaN, ignoring the new measurement: " << x << " " << y << " " << z ); return; } measurement_(1) = x; measurement_(2) = y; measurement_(3) = z; //we have received a new measurement meas_used_ = false; }
/** * @brief If not already mined, decrement the angular velocities.. */ void KeyOpCore::decrementAngularVelocity() { if (power_status) { if (cmd->angular.z >= -angular_vel_max) { cmd->angular.z -= angular_vel_step; } ROS_INFO_STREAM("KeyOp: angular velocity decremented [" << cmd->linear.x << "|" << cmd->angular.z << "]"); } else { ROS_WARN_STREAM("KeyOp: motors are not yet powered up."); } }
/** * Checks that we have ended inside the goal constraints. */ bool JointTrajectoryActionController::goalReached() { for (size_t i = 0; i < joints_.size(); i++) { double error = current_trajectory_->back().splines[i].target_position - katana_->getMotorAngles()[i]; if (goal_constraints_[i] > 0 && fabs(error) > goal_constraints_[i]) { ROS_WARN_STREAM("Joint " << i << " did not reach its goal. target position: " << current_trajectory_->back().splines[i].target_position << " actual position: " << katana_->getMotorAngles()[i] << std::endl); return false; } } return true; }
Mesh* createMeshFromResource(const std::string& resource, const Eigen::Vector3d &scale) { resource_retriever::Retriever retriever; resource_retriever::MemoryResource res; try { res = retriever.get(resource); } catch (resource_retriever::Exception& e) { ROS_ERROR("%s", e.what()); return NULL; } if (res.size == 0) { ROS_WARN("Retrieved empty mesh for resource '%s'", resource.c_str()); return NULL; } // Create an instance of the Importer class Assimp::Importer importer; // try to get a file extension std::string hint; std::size_t pos = resource.find_last_of("."); if (pos != std::string::npos) { hint = resource.substr(pos + 1); std::transform(hint.begin(), hint.end(), hint.begin(), ::tolower); if (hint.find("stl") != std::string::npos) hint = "stl"; } // And have it read the given file with some postprocessing const aiScene* scene = importer.ReadFileFromMemory(reinterpret_cast<void*>(res.data.get()), res.size, aiProcess_Triangulate | aiProcess_JoinIdenticalVertices | aiProcess_SortByPType | aiProcess_OptimizeGraph | aiProcess_OptimizeMeshes, hint.c_str()); if (!scene) { ROS_WARN_STREAM("Assimp reports no scene in " << resource); return NULL; } return createMeshFromAsset(scene, scale, resource); }
std::vector<geometry_msgs::PoseStamped> CartesianTrajectoryController::GetWayPoints(geometry_msgs::PoseStamped p1, geometry_msgs::PoseStamped p2) { ROS_ASSERT( p1.header.frame_id == p2.header.frame_id ); ROS_INFO( "Calculating SubWaypoint Positions" ); ROS_DEBUG_STREAM( "P1: " << p1 ); std::vector<geometry_msgs::PoseStamped> way_points; float distance = sqrt( pow( (p1.pose.position.x - p2.pose.position.x) ,2) + pow( (p1.pose.position.y - p2.pose.position.y) ,2) + pow( (p1.pose.position.z - p2.pose.position.z) ,2) ); //int number_of_way_points = ceil(distance/m_way_point_resolution); int number_of_way_points = 0; //double interval = 1 / number_of_way_points; double interval = 0.07; ROS_WARN_STREAM( "Calculated Interval" << " [" << interval << "]" ); double dx = (p2.pose.position.x - p1.pose.position.x); double dy = (p2.pose.position.y - p1.pose.position.y); double dz = (p2.pose.position.z - p1.pose.position.z); ROS_DEBUG_STREAM( "Calculated Slope" << " [" << dx << "," << dy << "," << dz << "]" ); way_points.push_back(p1); for (int i=0; i<number_of_way_points; i++) { geometry_msgs::PoseStamped next_point = way_points.at(i); next_point.pose.position.x += dx*interval; next_point.pose.position.y += dy*interval; next_point.pose.position.z += dz*interval; way_points.push_back(next_point); } way_points.push_back(p2); for( int x = 0; x < (int)way_points.size(); x ++ ) { ROS_DEBUG_STREAM( "SubWayPoint #" << x << " [" << way_points[x].pose.position.x << "," << way_points[x].pose.position.y << "," << way_points[x].pose.position.z << "]" ); } return way_points; }
void planning_environment::PlanningMonitor::getAllFixedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transform_vec) const { transform_vec.clear(); std::vector<std::string> all_frame_names; tf_->getFrameStrings(all_frame_names); //TODO - doesn't cope with tf namespaces //takes out leading slashes for(unsigned int i = 0; i < all_frame_names.size(); i++) { if(!all_frame_names[i].empty() && all_frame_names[i][0] == '/') { all_frame_names[i].erase(all_frame_names[i].begin()); } } //the idea here is that we need to figure out how to take poses from other frames //and transform them into the fixed frame. So we want to get the transform //that goes from the frame to the identity of the world frame and take the inverse for(unsigned int i = 0; i < all_frame_names.size(); i++) { if(all_frame_names[i] != cm_->getWorldFrameId() && !cm_->getKinematicModel()->hasLinkModel(all_frame_names[i])) { ROS_DEBUG_STREAM("Adding fixed frame transform for frame " << all_frame_names[i]); geometry_msgs::PoseStamped ident_pose; ident_pose.header.frame_id = cm_->getWorldFrameId(); ident_pose.pose.orientation.w = 1.0; std::string err_string; if (tf_->getLatestCommonTime(cm_->getWorldFrameId(), all_frame_names[i], ident_pose.header.stamp, &err_string) != tf::NO_ERROR) { ROS_WARN_STREAM("No transform whatsoever available between " << all_frame_names[i] << " and fixed frame" << cm_->getWorldFrameId()); continue; } geometry_msgs::PoseStamped trans_pose; try { tf_->transformPose(all_frame_names[i], ident_pose, trans_pose); } catch(tf::TransformException& ex) { //just not going to cache this one //ROS_WARN_STREAM("Unable to transform object from frame " << all_frame_names[i] << " to fixed frame " // << cm_->getWorldFrameId() << " tf error is " << ex.what()); continue; } geometry_msgs::TransformStamped f; f.header = ident_pose.header; f.child_frame_id = all_frame_names[i]; f.transform.translation.x = trans_pose.pose.position.x; f.transform.translation.y = trans_pose.pose.position.y; f.transform.translation.z = trans_pose.pose.position.z; f.transform.rotation = trans_pose.pose.orientation; transform_vec.push_back(f); } } }
bool planning_models::KinematicState::areJointsWithinBounds(const std::vector<std::string>& joints) const { for(std::vector<std::string>::const_iterator it = joints.begin(); it != joints.end(); it++) { const JointState* joint_state = getJointState(*it); if(joint_state == NULL) { ROS_WARN_STREAM("No joint with name " << *it); return false; } if(!joint_state->areJointStateValuesWithinBounds()) { return false; } } return true; }
bool endGame(pacman_msgs::EndGame::Request &req, pacman_msgs::EndGame::Response &res, ros::ServiceClient *start_game_client, DeterministicGameState **game_state) { // count number of games static int game_count = 0; game_count++; if (req.win) ROS_INFO_STREAM("Won game " << game_count); else ROS_WARN_STREAM("Lost game " << game_count); if (game_count < NUMBER_OF_GAMES) { pacman_msgs::StartGame start_game; if (game_count < NUMBER_OF_TRAININGS) start_game.request.show_gui = false; else { start_game.request.show_gui = true; is_training = false; } if (start_game_client->call(start_game)) if(start_game.response.started) { // new game started delete *game_state; *game_state = new DeterministicGameState(); res.game_restarted = true; ROS_INFO("New game started"); return true; } else ROS_ERROR("Failed to start game (check if game already started)"); else // if problem => print error ROS_ERROR("Failed to call service StartGame"); } else ROS_INFO_STREAM("Game finished, type ctrl+c to exit"); // game not restarted res.game_restarted = false; return true; }
void StompTrajectory::overwriteTrajectory(const trajectory_msgs::JointTrajectory& traj) { std::vector<int> ind; for(unsigned int j = 0; j < traj.joint_names.size(); j++) { int kdl_number = robot_model_->urdfNameToKdlNumber(traj.joint_names[j]); if(kdl_number == 0) { ROS_WARN_STREAM("Can't find kdl index for joint " << traj.joint_names[j]); } ind.push_back(kdl_number); } for(unsigned int i = 1; i <= traj.points.size(); i++) { for(unsigned int j = 0; j < traj.joint_names.size(); j++) { trajectory_(i,ind[j]) = traj.points[i-1].positions[j]; } } }
MapMakerServerBase::MapMakerServerBase(Map& map, TaylorCameraMap& cameras, BundleAdjusterBase& bundleAdjuster) : MapMakerBase(map, true) // This will be skipped since inheritance is virtual! , mmCameraModels(cameras) , mBundleAdjuster(bundleAdjuster) { // This needs to be remapped in the launch file mResetSystemClient = mNodeHandle.serviceClient<mcptam::Reset>("reset", true); ROS_INFO_STREAM("MapMakerServerBase: Reset system client is connecting to: " << mResetSystemClient.getService()); while (!mResetSystemClient.waitForExistence(ros::Duration(5)) && ros::ok()) { ROS_WARN_STREAM("MapMakerServerBase: Waiting for system reset service to be advertised..."); } Reset(); };
/** Read camera data. * * @param image points to camera Image message * @return true if successful, with image filled in */ bool NaoqiCameraDriver::read(sensor_msgs::ImagePtr &image) { bool success = true; try { // Read data from the Camera ROS_DEBUG_STREAM("[" << camera_name_ << "] reading data"); //TODO: support local image access. This first suppose // to write a MAOqi 'local' module. // cf: http://www.aldebaran-robotics.com/documentation/dev/cpp/tutos/create_module.html#how-to-create-a-local-module ALValue al_image = camera_proxy_->getImageRemote(camera_name_); if (config_.use_ros_time) image->header.stamp = ros::Time::now(); else { // use NAOqi timestamp image->header.stamp = ros::Time(((double) al_image[4] / 1000000.0) + (double) al_image[5]); } image->width = (int) al_image[0]; image->height = (int) al_image[1]; image->step = image->width * (int) al_image[2]; image->encoding = sensor_msgs::image_encodings::RGB8; int image_size = image->height * image->step; image->data.resize(image_size); memcpy(&(image->data)[0], al_image[6].GetBinary(), image_size); camera_proxy_->releaseImage(camera_name_); success = true; ROS_DEBUG_STREAM("[" << camera_name_ << "] read returned"); } catch (naoqicamera_driver::Exception& e) { ROS_WARN_STREAM("[" << camera_name_ << "] Exception reading data: " << e.what()); success = false; } return success; }
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::OrientationConstraint::decide(const planning_models::KinematicState &state, bool verbose) const { if (!link_model_) return ConstraintEvaluationResult(true, 0.0); const planning_models::KinematicState::LinkState *link_state = state.getLinkState(link_model_->getName()); if (!link_state) { ROS_WARN_STREAM("No link in state with name '" << link_model_->getName() << "'"); return ConstraintEvaluationResult(false, 0.0); } Eigen::Vector3d xyz; if (mobile_frame_) { Eigen::Matrix3d tmp; tf_->transformRotationMatrix(state, desired_rotation_frame_id_, desired_rotation_matrix_, tmp); Eigen::Affine3d diff(tmp.inverse() * link_state->getGlobalLinkTransform().rotation()); xyz = diff.rotation().eulerAngles(0, 1, 2); // XYZ } else { Eigen::Affine3d diff(desired_rotation_matrix_inv_ * link_state->getGlobalLinkTransform().rotation()); xyz = diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to ZXZ, the convention used in sampling constraints } xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0))); xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1))); xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2))); bool result = xyz(2) < absolute_z_axis_tolerance_ && xyz(1) < absolute_y_axis_tolerance_ && xyz(0) < absolute_x_axis_tolerance_; if (verbose) { Eigen::Quaterniond q_act(link_state->getGlobalLinkTransform().rotation()); Eigen::Quaterniond q_des(desired_rotation_matrix_); ROS_INFO("Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f", result ? "satisfied" : "violated", link_model_->getName().c_str(), q_des.x(), q_des.y(), q_des.z(), q_des.w(), q_act.x(), q_act.y(), q_act.z(), q_act.w(), xyz(0), xyz(1), xyz(2), absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_); } return ConstraintEvaluationResult(result, constraint_weight_ * (xyz(0) + xyz(1) + xyz(2))); }
bool PlaceObject::MoveToPose(const geometry_msgs::PoseStamped& target_pose, double eef_step, bool check_collision) { // move_group_->setJointValueTarget(target_pose); target_pose_publisher_.publish(target_pose); geometry_msgs::PoseStamped current_pose = move_group_->getCurrentPose(); std::vector<geometry_msgs::Pose> waypoints; waypoints.push_back(current_pose.pose); waypoints.push_back(target_pose.pose); robot_state::RobotState rs = *move_group_->getCurrentState(); move_group_->setStartState(rs); // Plan trajectory. moveit_msgs::RobotTrajectory trajectory; moveit_msgs::MoveItErrorCodes error_code; double path_fraction = move_group_->computeCartesianPath(waypoints, eef_step, 0.0, trajectory, check_collision, &error_code); if(path_fraction < 1.0) { ROS_WARN_STREAM("PlacesObject: Could not calculate Cartesian Path."); return false; } robot_trajectory::RobotTrajectory robot_trajectory(rs.getRobotModel(), move_group_->getName()); robot_trajectory.setRobotTrajectoryMsg(rs, trajectory); trajectory_processing::IterativeParabolicTimeParameterization iptp; iptp.computeTimeStamps(robot_trajectory, 1.0); robot_trajectory.getRobotTrajectoryMsg(trajectory); moveit::planning_interface::MoveGroup::Plan my_plan; moveit_msgs::RobotState robot_state_msg; robot_state::robotStateToRobotStateMsg(*move_group_->getCurrentState(), robot_state_msg); my_plan.trajectory_ = trajectory; my_plan.start_state_ = robot_state_msg; // removing points with velocity zero my_plan.trajectory_.joint_trajectory.points.erase( std::remove_if(my_plan.trajectory_.joint_trajectory.points.begin() + 1, my_plan.trajectory_.joint_trajectory.points.end(), [](const trajectory_msgs::JointTrajectoryPoint & p) { return p.time_from_start.toSec() == 0;}), my_plan.trajectory_.joint_trajectory.points.end()); move_group_->execute(my_plan); return true; }
MarkersConfig::MarkersConfig(std::string path, float default_val)throw(cv::Exception): msize(__number_of_possible_markers,default_val), mposition(__number_of_possible_markers,cv::Point3f(0,0,0)) { std::ifstream file(path.c_str()); if(!file) throw cv::Exception(9100, "Could not open file:"+path,"::constructor::",__FILE__,__LINE__); std::list<int> readIDs; char line[1024]; while(!file.eof()){ file.getline(line,1024); int id; float size,m_x,m_y,m_z; switch (sscanf(line,"%d = %f (%f,%f,%f)",&id,&size,&m_x,&m_y,&m_z)){ case 2: if(this->msize.at(id) != default_val){ ROS_WARN_STREAM("Warning: ID Marker " << id << " is " << size << " instead of the previous " << this->msize.at(id)); } this->msize.at(id)=size; readIDs.push_back(id); //debug: ROS_INFO_STREAM(id << " = " << size ); break; case 5: if(this->msize.at(id) != default_val){ std::cout << "Warning: ID Marker " << id << " is " << size << " instead of the previous " << this->msize.at(id) << std::endl; std::cout << "Probably you want to review the file configuration: " << std::endl; } this->msize.at(id)=size; this->mposition.at(id)= cv::Point3f(m_x,m_y,m_z); readIDs.push_back(id); //debug: ROS_INFO_STREAM(id << " = " << size << " (" << m_x << ',' << m_y << ',' << m_z << ')'); break; } } }
void rosNamed(const std::vector<std::string> &msgs) { if (msgs.size()==0) return; if (msgs.size()==1) { ROS_INFO_STREAM("Kobuki : " << msgs[0]); } if (msgs.size()==2) { if (msgs[0] == "debug") { ROS_DEBUG_STREAM("Kobuki : " << msgs[1]); } else if (msgs[0] == "info" ) { ROS_INFO_STREAM ("Kobuki : " << msgs[1]); } else if (msgs[0] == "warn" ) { ROS_WARN_STREAM ("Kobuki : " << msgs[1]); } else if (msgs[0] == "error") { ROS_ERROR_STREAM("Kobuki : " << msgs[1]); } else if (msgs[0] == "fatal") { ROS_FATAL_STREAM("Kobuki : " << msgs[1]); } } if (msgs.size()==3) { if (msgs[0] == "debug") { ROS_DEBUG_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); } else if (msgs[0] == "info" ) { ROS_INFO_STREAM_NAMED (msgs[1], "Kobuki : " << msgs[2]); } else if (msgs[0] == "warn" ) { ROS_WARN_STREAM_NAMED (msgs[1], "Kobuki : " << msgs[2]); } else if (msgs[0] == "error") { ROS_ERROR_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); } else if (msgs[0] == "fatal") { ROS_FATAL_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); } } }
/** Read camera data. * * @return true if successful */ bool read() { bool success = true; try { // Read data from the Camera ROS_DEBUG_STREAM("[" << camera_name_ << "] reading data"); dev_->readData(image_); ROS_DEBUG_STREAM("[" << camera_name_ << "] read returned"); } catch (camera1394v2::Exception& e) { ROS_WARN_STREAM("[" << camera_name_ << "] Exception reading data: " << e.what()); success = false; } return success; }
/** Read camera data. * * @param image points to camera Image message * @return true if successful, with image filled in */ bool Camera1394Driver::read(sensor_msgs::ImagePtr &image) { bool success = true; try { // Read data from the Camera ROS_DEBUG_STREAM("[" << camera_name_ << "] reading data"); success = dev_->readData(*image); ROS_DEBUG_STREAM("[" << camera_name_ << "] read returned"); } catch (camera1394::Exception& e) { ROS_WARN_STREAM("[" << camera_name_ << "] Exception reading data: " << e.what()); success = false; } return success; }