void spin() { // initialize random seed srand(time(NULL)); // dynamics model boost::shared_ptr<DynamicModel > dyn_model( new DynModelPlanar5() ); std::string robot_description_str; std::string robot_semantic_description_str; nh_.getParam("/robot_description", robot_description_str); nh_.getParam("/robot_semantic_description", robot_semantic_description_str); // // collision model // boost::shared_ptr<self_collision::CollisionModel> col_model = self_collision::CollisionModel::parseURDF(robot_description_str); col_model->parseSRDF(robot_semantic_description_str); col_model->generateCollisionPairs(); // external collision objects - part of virtual link connected to the base link self_collision::Link::VecPtrCollision col_array; col_array.push_back( self_collision::createCollisionCapsule(0.2, 0.3, KDL::Frame(KDL::Rotation::RotX(90.0/180.0*PI), KDL::Vector(1, 0.5, 0))) ); if (!col_model->addLink("env_link", "base", col_array)) { ROS_ERROR("ERROR: could not add external collision objects to the collision model"); return; } col_model->generateCollisionPairs(); // // robot state // std::vector<std::string > joint_names; joint_names.push_back("0_joint"); joint_names.push_back("1_joint"); joint_names.push_back("2_joint"); joint_names.push_back("3_joint"); joint_names.push_back("4_joint"); int ndof = joint_names.size(); Eigen::VectorXd q, dq, ddq, torque; q.resize( ndof ); dq.resize( ndof ); ddq.resize( ndof ); torque.resize( ndof ); for (int q_idx = 0; q_idx < ndof; q_idx++) { q[q_idx] = -0.1; dq[q_idx] = 0.0; ddq[q_idx] = 0.0; torque[q_idx] = 0.0; } std::string effector_name = "effector"; int effector_idx = col_model->getLinkIndex(effector_name); // // kinematic model // boost::shared_ptr<KinematicModel> kin_model( new KinematicModel(robot_description_str, joint_names) ); KinematicModel::Jacobian J_r_HAND_6, J_r_HAND; J_r_HAND_6.resize(6, ndof); J_r_HAND.resize(3, ndof); std::vector<KDL::Frame > links_fk(col_model->getLinksCount()); // joint limits Eigen::VectorXd lower_limit(ndof), upper_limit(ndof), limit_range(ndof), max_trq(ndof); int q_idx = 0; for (std::vector<std::string >::const_iterator name_it = joint_names.begin(); name_it != joint_names.end(); name_it++, q_idx++) { lower_limit[q_idx] = kin_model->getLowerLimit(q_idx); upper_limit[q_idx] = kin_model->getUpperLimit(q_idx); limit_range[q_idx] = 10.0/180.0*PI; max_trq[q_idx] = 10.0; } Eigen::VectorXd max_q(ndof); max_q(0) = 10.0/180.0*PI; max_q(1) = 20.0/180.0*PI; max_q(2) = 20.0/180.0*PI; max_q(3) = 30.0/180.0*PI; max_q(4) = 40.0/180.0*PI; boost::shared_ptr<DynamicsSimulatorHandPose> sim( new DynamicsSimulatorHandPose(ndof, 6, effector_name, col_model, kin_model, dyn_model, joint_names, max_q) ); /* // // Tasks declaration // Task_JLC task_JLC(lower_limit, upper_limit, limit_range, max_trq); Task_WCC task_WCC(ndof, 3, 4); double activation_dist = 0.05; Task_COL task_COL(ndof, activation_dist, 10.0, kin_model, col_model); Task_HAND task_HAND(ndof, 3); */ /* while (ros::ok()) { // // wrist collision constraint // Eigen::VectorXd torque_WCC(ndof); Eigen::MatrixXd N_WCC(ndof, ndof); task_WCC.compute(q, dq, dyn_model->getM(), dyn_model->getInvM(), torque_WCC, N_WCC, markers_pub_); markers_pub_.publish(); ros::spinOnce(); char ch = getchar(); if (ch == 'a') { q(3) -= 0.1; } else if (ch == 'd') { q(3) += 0.1; } else if (ch == 's') { q(4) -= 0.1; } else if (ch == 'w') { q(4) += 0.1; } } return; */ // loop variables ros::Time last_time = ros::Time::now(); KDL::Frame r_HAND_target; int loop_counter = 10000; ros::Rate loop_rate(100); while (true) { generatePossiblePose(r_HAND_target, q, ndof, effector_name, col_model, kin_model); sim->setState(q, dq, ddq); sim->setTarget(r_HAND_target); sim->oneStep(); if (!sim->inCollision()) { break; } } while (ros::ok()) { if (loop_counter > 500) { Eigen::VectorXd q_tmp(ndof); generatePossiblePose(r_HAND_target, q_tmp, ndof, effector_name, col_model, kin_model); sim->setTarget(r_HAND_target); publishTransform(br, r_HAND_target, "effector_dest", "base"); loop_counter = 0; } loop_counter += 1; sim->oneStep(&markers_pub_, 3000); /* if (sim->inCollision() && !collision_in_prev_step) { collision_in_prev_step = true; std::cout << "collision begin" << std::endl; } else if (!sim->inCollision() && collision_in_prev_step) { collision_in_prev_step = false; std::cout << "collision end" << std::endl; } */ sim->getState(q, dq, ddq); // publish markers and robot state with limited rate ros::Duration time_elapsed = ros::Time::now() - last_time; if (time_elapsed.toSec() > 0.05) { // calculate forward kinematics for all links for (int l_idx = 0; l_idx < col_model->getLinksCount(); l_idx++) { kin_model->calculateFk(links_fk[l_idx], col_model->getLinkName(l_idx), q); } publishJointState(joint_state_pub_, q, joint_names); int m_id = 0; m_id = addRobotModelVis(markers_pub_, m_id, col_model, links_fk); markers_pub_.publish(); ros::Time last_time = ros::Time::now(); } ros::spinOnce(); loop_rate.sleep(); } }
bool SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan) { // Get the laser's pose, relative to base. tf::Stamped<tf::Pose> ident; tf::Stamped<tf::Transform> laser_pose; ident.setIdentity(); ident.frame_id_ = scan.header.frame_id; ident.stamp_ = scan.header.stamp; try { tf_.transformPose(base_frame_, ident, laser_pose); #ifdef LOGTOFILE writeLaserPoseStamped(laser_pose); #endif } catch(tf::TransformException e) { ROS_WARN("Failed to compute laser pose, aborting initialization (%s)", e.what()); return false; } double yaw = tf::getYaw(laser_pose.getRotation()); GMapping::OrientedPoint gmap_pose(laser_pose.getOrigin().x(), laser_pose.getOrigin().y(), yaw); ROS_DEBUG("laser's pose wrt base: %.3f %.3f %.3f", laser_pose.getOrigin().x(), laser_pose.getOrigin().y(), yaw); // To account for lasers that are mounted upside-down, we determine the // min, max, and increment angles of the laser in the base frame. tf::Quaternion q; q.setRPY(0.0, 0.0, scan.angle_min); tf::Stamped<tf::Quaternion> min_q(q, scan.header.stamp, scan.header.frame_id); q.setRPY(0.0, 0.0, scan.angle_max); tf::Stamped<tf::Quaternion> max_q(q, scan.header.stamp, scan.header.frame_id); try { tf_.transformQuaternion(base_frame_, min_q, min_q); tf_.transformQuaternion(base_frame_, max_q, max_q); } catch(tf::TransformException& e) { ROS_WARN("Unable to transform min/max laser angles into base frame: %s", e.what()); return false; } #ifdef LOGTOFILE writeMinMax(min_q); writeMinMax(max_q); #endif gsp_laser_beam_count_ = scan.ranges.size(); double angle_min = tf::getYaw(min_q); double angle_max = tf::getYaw(max_q); gsp_laser_angle_increment_ = scan.angle_increment; ROS_DEBUG("Laser angles in base frame: min: %.3f max: %.3f inc: %.3f", angle_min, angle_max, gsp_laser_angle_increment_); // setting maxRange and maxUrange here so we can set a reasonable default ros::NodeHandle private_nh_("~"); if(!private_nh_.getParam("maxRange", maxRange_)) maxRange_ = scan.range_max - 0.01; if(!private_nh_.getParam("maxUrange", maxUrange_)) maxUrange_ = maxRange_; // The laser must be called "FLASER". // We pass in the absolute value of the computed angle increment, on the // assumption that GMapping requires a positive angle increment. If the // actual increment is negative, we'll swap the order of ranges before // feeding each scan to GMapping. gsp_laser_ = new GMapping::RangeSensor("FLASER", gsp_laser_beam_count_, fabs(gsp_laser_angle_increment_), gmap_pose, 0.0, maxRange_); ROS_ASSERT(gsp_laser_); GMapping::SensorMap smap; smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_)); gsp_->setSensorMap(smap); gsp_odom_ = new GMapping::OdometrySensor(odom_frame_); ROS_ASSERT(gsp_odom_); /// @todo Expose setting an initial pose GMapping::OrientedPoint initialPose; if(!getOdomPose(initialPose, scan.header.stamp)) initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0); gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_, kernelSize_, lstep_, astep_, iterations_, lsigma_, ogain_, lskip_); gsp_->setMotionModelParameters(srr_, srt_, str_, stt_); gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_); gsp_->setUpdatePeriod(temporalUpdate_); gsp_->setgenerateMap(false); gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_, delta_, initialPose); gsp_->setllsamplerange(llsamplerange_); gsp_->setllsamplestep(llsamplestep_); /// @todo Check these calls; in the gmapping gui, they use /// llsamplestep and llsamplerange intead of lasamplestep and /// lasamplerange. It was probably a typo, but who knows. gsp_->setlasamplerange(lasamplerange_); gsp_->setlasamplestep(lasamplestep_); // Call the sampling function once to set the seed. GMapping::sampleGaussian(1,time(NULL)); ROS_INFO("Initialization complete"); return true; }
karto::LaserRangeFinder* SlamKarto::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan) { // Check whether we know about this laser yet if(lasers_.find(scan->header.frame_id) == lasers_.end()) { // New laser; need to create a Karto device for it. // Get the laser's pose, relative to base. tf::Stamped<tf::Pose> ident; tf::Stamped<tf::Transform> laser_pose; ident.setIdentity(); ident.frame_id_ = scan->header.frame_id; ident.stamp_ = scan->header.stamp; try { tf_.transformPose(base_frame_, ident, laser_pose); } catch(tf::TransformException e) { ROS_WARN("Failed to compute laser pose, aborting initialization (%s)", e.what()); return NULL; } double yaw = tf::getYaw(laser_pose.getRotation()); ROS_INFO("laser %s's pose wrt base: %.3f %.3f %.3f", scan->header.frame_id.c_str(), laser_pose.getOrigin().x(), laser_pose.getOrigin().y(), yaw); // To account for lasers that are mounted upside-down, we determine the // min, max, and increment angles of the laser in the base frame. tf::Quaternion q; q.setRPY(0.0, 0.0, scan->angle_min); tf::Stamped<tf::Quaternion> min_q(q, scan->header.stamp, scan->header.frame_id); q.setRPY(0.0, 0.0, scan->angle_max); tf::Stamped<tf::Quaternion> max_q(q, scan->header.stamp, scan->header.frame_id); try { tf_.transformQuaternion(base_frame_, min_q, min_q); tf_.transformQuaternion(base_frame_, max_q, max_q); } catch(tf::TransformException& e) { ROS_WARN("Unable to transform min/max laser angles into base frame: %s", e.what()); return false; } double angle_min = tf::getYaw(min_q); double angle_max = tf::getYaw(max_q); bool inverse = lasers_inverted_[scan->header.frame_id] = angle_max < angle_min; if (inverse) ROS_INFO("laser is mounted upside-down"); // Create a laser range finder device and copy in data from the first // scan std::string name = scan->header.frame_id; karto::LaserRangeFinder* laser = karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, karto::Name(name)); laser->SetOffsetPose(karto::Pose2(laser_pose.getOrigin().x(), laser_pose.getOrigin().y(), yaw)); laser->SetMinimumRange(scan->range_min); laser->SetMaximumRange(scan->range_max); laser->SetMinimumAngle(scan->angle_min); laser->SetMaximumAngle(scan->angle_max); laser->SetAngularResolution(scan->angle_increment); // TODO: expose this, and many other parameters //laser_->SetRangeThreshold(12.0); // Store this laser device for later lasers_[scan->header.frame_id] = laser; // Add it to the dataset, which seems to be necessary dataset_->Add(laser); } return lasers_[scan->header.frame_id]; }