//Initialize parameters WF_Agent::WF_Agent(ros::NodeHandle nh): n(nh) //ros::NodeHandle n = nh; { //accessing private parameters ros::NodeHandle n_private("~"); /*linAxis, angAxis variables are used to define which axes of the *joystick will control the pioneer robot. Also, the parameter server * was checked for new scalar values for driving the robot. */ // n_private.param("linAxis", linAxis, 1); // n_private.param("angAxis", angAxis, 0); n_private.param("vel_topic", vel_topic, std::string("cmd_vel")); // n_private.param("odom_topic", odom_topic, std::string("odom")); // n_private.param("laser_topic", laser_topic, std::string("base_scan")); n_private.param("odom_topic", odom_topic, std::string("pose")); n_private.param("laser_topic", laser_topic, std::string("scan")); n_private.param("dist_topic", dist_topic, std::string("dist")); n_private.param("motor_state_topic", motor_state_topic, std::string("cmd_motor_state")); //pioneer stuff /*creating a publisher that will advertise the cmd_vel(command velocity topic of the pioneer robot. */ vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel",1); vis_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 0); mot_pub = n.advertise<p2os_driver::MotorState>(motor_state_topic.c_str(),1); //pioneer stuff /*subscribing to the joystick topic for the input to drive the robot *If the node is slow in processing incoming messages on the joystick topic, * up to 10 messages will be buffered before any are lost. */ odom_sub = n.subscribe<nav_msgs::Odometry>(odom_topic.c_str(), 10, &WF_Agent::odom_cb, this); laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_topic.c_str(), 10, &WF_Agent::laser_cb, this); wall_dist_sub = n.subscribe<wf::Dist>(dist_topic.c_str(), 10, &WF_Agent::dist_cb, this); }
Experiment::Experiment(ros::NodeHandle n):n_(n) { ros::NodeHandle n_private("~"); n_private.param("num_reps", num_reps_, 1000); n_private.param("freq", freq_, 1.0); n_private.param("start_x", start_x_, -5.0); n_private.param("start_y", start_y_, 5.0); n_private.param("goal_radius", radius_, 0.5); n_private.param("learn", learn_, true); n_private.param("learning_rate", learning_rate_, 0.5); n_private.param("discount_factor", discount_factor_, 0.5); n_private.param("max_explore", max_explore_, 8); cnt_rep_ = 0; move_stopped_ = false; mode_ = MODE_REP_START; states_ = new States(n); actions_ = new Actions(n); qobj_ = new QLearner(actions_->GetNumActions(), states_->GetNumStates(), learning_rate_, discount_factor_, learn_, max_explore_); bool_sub_ = n.subscribe("/move_done", 1, &Experiment::bool_cb, this); // ros::Subscriber odom_sub_ = n.subscribe("/odom", 10, odom_cb); client_ = n.serviceClient<stage_light_ml::move_robot>("/move_robot"); timer_ = n.createTimer(ros::Duration(1/freq_), &Experiment::timer_cb, this); }
KeyboardTeleopNode() { pub_ = n_.advertise<sensor_msgs::JointState>("/joint_states", 1000); cmdjs_.name.clear(); cmdjs_.name.push_back("joint_1"); cmdjs_.name.push_back("joint_2"); cmdjs_.name.push_back("joint_3"); cmdjs_.name.push_back("joint_4"); cmdjs_.name.push_back("joint_5"); cmdjs_.name.push_back("joint_6"); cmdjs_.position.clear(); for(int i=0;i<6;++i){ cmdjs_.position.push_back(0.0f); } // needed header time info, or the rviz can not display the model states cmdjs_.header.stamp= ros::Time::now(); pub_.publish(cmdjs_); ros::NodeHandle n_private("~"); }
int main(int argc,char ** argv){ ros::init(argc, argv, "ml_ndt"); ros::NodeHandle n; ros::NodeHandle n_private("~"); MlNdt ndt(n,n_private); ros::spin(); return 0; }
KeyboardTeleopArm::KeyboardTeleopArm() { ros::NodeHandle n_private("~"); n_private.param("joint_angular_step", joint_angular_step_, 0.0174); n_private.param("gripper_angular_step", gripper_angular_step_, 0.1); joint_pos_pub_ = xm_nh_.advertise<xm_msgs::xm_JointPos>("joint_pos_cmd", 1000); gripper_pos_pub_ = xm_nh_.advertise<std_msgs::Float64>("gripper_joint/command", 1000); }
ErraticKeyboardTeleopNode() { pub_ = n_.advertise<geometry_msgs::Twist> ("cmd_vel", 1); ros::NodeHandle n_private ("~"); n_private.param ("walk_vel", walk_vel_, 0.4); n_private.param ("run_vel", run_vel_, 0.7); n_private.param ("yaw_rate", yaw_rate_, 0.4); n_private.param ("yaw_rate_run", yaw_rate_run_, 0.8); }
void init() { nh_.param("scale_angular", a_scale_, 1.0); nh_.param("scale_linear", l_scale_, 0.23); nh_.param("walk_scale", walk_scale_, 0.6); vel_pub_ = nh_.advertise<geometry_msgs::Twist> ("cmd_vel", 1); ros::NodeHandle n_private("~"); }
void init() { cmd.data = 0; vel_pub_r_ = n_.advertise<std_msgs::Float64>("/wsg_50_gr/command", 1); vel_pub_l_ = n_.advertise<std_msgs::Float64>("/wsg_50_gl/command", 1); ros::NodeHandle n_private("~"); n_private.param("open_increment", open_increment, 0.001); }
ErraticKeyboardTeleopNode() { pub_ = n_.advertise<geometry_msgs::Twist>("desired_cmd_vel", 1); //pub_ = n_.advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1); ros::NodeHandle n_private("~"); n_private.param("walk_vel", walk_vel_, 0.5); n_private.param("run_vel", run_vel_, 1.0); n_private.param("yaw_rate", yaw_rate_, 1.0); n_private.param("yaw_rate_run", yaw_rate_run_, 1.5); }
QLearner::QLearner(ros::NodeHandle nh) { n_ = nh; ros::NodeHandle n_private("~"); n_private.param("num_states", num_states_, 8); n_private.param("num_actions", num_actions_, 3); n_private.param("learning_rate", learning_rate_, 0.3); n_private.param("discount_factor", discount_factor_, 0.5); n_private.param("temp_const", temp_const_, 5.0); n_private.param("temp_alpha", temp_alpha_, 0.85); temp_cnt_ = 0; temp_ = temp_const_ * pow(temp_alpha_, temp_cnt_); size_array_ = num_actions_ * num_states_; srand ( time(NULL) ); Init(); if (n_private.hasParam("qarray")) { learn_ = false; // Get the qtable XmlRpc::XmlRpcValue qtable; n_private.getParam("qarray", qtable); if (qtable.getType() != XmlRpc::XmlRpcValue::TypeArray) ROS_ERROR("Error reading footsteps/x from config file."); int size; try { size = qtable.size(); if (size != (num_states_ * num_actions_)) { ROS_ERROR("Size of array does not match num_states * num_actions = %d,\ exiting.", num_states_ * num_actions_); exit(0); } } catch (const XmlRpc::XmlRpcException e) { ROS_ERROR("No table available, exiting."); exit(0); } // create qarray set for(int i=0; i < size; i++) { q_array_.push_back((double)qtable[i]); } }
MoveSimple::MoveSimple(ros::NodeHandle nh):n_(nh) { ros::NodeHandle n_private("~"); n_private.param("max_lin_vel", max_lin_vel_, 0.5); n_private.param("max_ang_vel", max_ang_vel_, 1.0); move_cmd_sub_ = n_.subscribe("move_cmd", 1, &MoveSimple::cb_cmd, this); odom_sub_ = n_.subscribe("base_pose_ground_truth", 1, &MoveSimple::cb_odom, this); vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1); move_done_pub_ = n_.advertise<std_msgs::Bool>("move_done", 1); state_ = MOVE_NONE; }
void init() { cmd.linear.x = cmd.linear.y = cmd.angular.z = 0; vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1); ros::NodeHandle n_private("~"); n_private.param("walk_vel", walk_vel, 0.5); n_private.param("run_vel", run_vel, 1.0); n_private.param("yaw_rate", yaw_rate, 1.0); n_private.param("yaw_run_rate", yaw_rate_run, 1.5); }
ManualCalibration() { ros::NodeHandle n_private("~"); n_private.param("walk_vel", walk_vel_, 0.5); n_private.param("run_vel", run_vel_, 1.0); n_private.param("yaw_rate", yaw_rate_, 1.0); n_private.param("yaw_rate_run", yaw_rate_run_, 1.5); tx=-0.3483; ty=-0.01316162; tz=0.643236; tf::Quaternion quat(0.228405, 0.326475, -0.502927, 0.767014); tf::Matrix3x3(quat).getRPY(roll, pitch, yaw); }
void init() { cmd.linear.x = cmd.linear.y = cmd.angular.z = 0; gripper.data = 0; joy_sub_ = n_.subscribe<sensor_msgs::Joy>("/teleop_joy",10,&TeleopUAVJoy::JoyCallback,this); vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1); grip_pub_ = n_.advertise<std_msgs::Float64>("/r_gripper_controller/command",1); if(!n_.getParam("teleopUGV",teleopUGV)) puts("fail to load the param"); ros::NodeHandle n_private("~"); n_private.param("walk_vel", walk_vel, 1.0); n_private.param("run_vel", run_vel, 8.0); n_private.param("yaw_rate", yaw_rate, 0.5); n_private.param("yaw_run_rate", yaw_rate_run, 1.5); n_private.param("vertical_vel", vertical_vel, 2.0); }
int main(int argc, char **argv) { ros::init(argc, argv, "PCL_Updater"); ros::NodeHandle n; ros::NodeHandle n_private("~"); n_private.param("period", period, 0.1); ros::Subscriber PCLSub = n.subscribe("/cloud_out", 1000, PCL_callback); ros::Timer timer = n.createTimer(ros::Duration(period), RefreshData); PCL_Pub = n.advertise<sensor_msgs::PointCloud>("/Assembled_PCL", 10);//Publish PointCloud PCL2_Pub = n.advertise<sensor_msgs::PointCloud2>("/Assembled_PCL2", 10);//Publish PointCloud2 ros::spin(); return 0; }
void KeyboardTeleoperation::init() { baseCommand.linear.x = 0; baseCommand.linear.y = 0; baseCommand.angular.z = 0; baseCommandPublisher = n.advertise<geometry_msgs::Twist>("cmd_vel", 1); baseOdometrySubscriber = n.subscribe("odom", 1, &KeyboardTeleoperation::baseOdometryCallback, this); // subscribing for joint states ros::NodeHandle n_private("~"); n_private.param("translationalSpeed", translationalSpeed, 0.1); n_private.param("rotationalSpeed", rotationalSpeed, 0.1); n_private.param("incrementRate", incrementRate, 0.1); n_private.param("maxTraveledDistance", maxTraveledDistance, 1.0); experimentNumber = 0; experimentIsRunning = false; }
void init() { //header - this is impt cmd.header.frame_id = "/torso_lift_link"; //Clear out our cmd - these values are roundabout initials cmd.pose.position.x=0.6; cmd.pose.position.y=-0.2; cmd.pose.position.z=-0.1; cmd.pose.orientation.x=-0.00244781865415; cmd.pose.orientation.y=-0.548220284495; cmd.pose.orientation.z=0.00145617884538; cmd.pose.orientation.w=0.836329126239; pose_pub_ = n_.advertise<geometry_msgs::PoseStamped>("r_cart/command", 1); ros::NodeHandle n_private("~"); }
LightSensor::LightSensor(ros::NodeHandle nh) { double xdist, ydist; n_ = nh; ros::NodeHandle n_private("~"); n_private.param("xdist", xdist, 8 / 0.0254); n_private.param("ydist", ydist, 6 / 0.0254); angle_ = atan2(ydist, xdist); sensor_hypotenuse_ = sqrt(xdist * xdist + ydist * ydist); flls_pub_ = n_.advertise<phidgets_ros::Float64Stamped>("flls", 1); frls_pub_ = n_.advertise<phidgets_ros::Float64Stamped>("frls", 1); rlls_pub_ = n_.advertise<phidgets_ros::Float64Stamped>("rlls", 1); rrls_pub_ = n_.advertise<phidgets_ros::Float64Stamped>("rrls", 1); odom_sub_ = n_.subscribe<nav_msgs::Odometry>("base_pose_ground_truth", 10, &LightSensor::cb_odom, this); timer_ = n_.createTimer(ros::Duration(0.02), &LightSensor::cb_tmr, this); }
Actions::Actions(ros::NodeHandle nh) { n_ = nh; ros::NodeHandle n_private("~"); n_private.param("num_actions", num_actions_, 3); n_private.param("ang_vel_lim", ang_vel_lim_, 1.0); n_private.param("lin_vel", lin_vel_, 0.3); vel_pub_ = n_.advertise<geometry_msgs::Twist>("/cmd_vel", 1); tmr_vel_ = n_.createTimer(ros::Duration(1 / 20), &Actions::timer_cb, this); ang_vels_ = std::vector<double>(num_actions_); ang_inc_ = 2 * ang_vel_lim_ / (num_actions_ - 1); for (int i = 0; i < num_actions_; i++) ang_vels_[i] = -ang_vel_lim_ + i * ang_inc_; vel_msg_.linear.x = 0.0; vel_msg_.angular.z = 0.0; }
TrackingApp::TrackingApp(const ros::NodeHandle& nodeh) { // Init attributes this->nh = nodeh; // Setup some ROS stuff statusSub = nh.subscribe("sensor_status", 1, &TrackingApp::statusCallback, this); cmdPub = nh.advertise<std_msgs::UInt16>("cmd_mode", 1); // Parameters ros::NodeHandle n_private("~"); int tmpTime; n_private.param("wantedMowingTime", tmpTime, 60 * 60); ROS_INFO("Param: wantedMowingTime: [%d]", tmpTime); wantedMowingTime = ros::Duration(tmpTime); state = HVA_TRK_STATE_IDLE; logCounter = 0; }
void init(){ ls_sub_ = n_.subscribe("ls_info", 1, &TeleopUAVController::laserInfoCallback, this); cmd.linear.x = cmd.linear.y = cmd.angular.z = 0; vel_pub_ = n_.advertise<geometry_msgs::Twist>("/uav1/cmd_vel", 1); ros::NodeHandle n_private("~"); n_private.param("walk_vel", walk_vel, 1.0); n_private.param("run_vel", run_vel, 1.0); n_private.param("yaw_rate", yaw_rate, 1.0); n_private.param("yaw_run_rate", yaw_rate_run, 1.5); walk_vel = 1.0; run_vel = 1.0; yaw_rate= 1.0; yaw_rate_run = 1.5; number = 0; }
WheelMap::WheelMap(const ros::NodeHandle& nodeh) { // Init attributes this->nh = nodeh; // Setup some ROS stuff map_pub = nh.advertise<nav_msgs::OccupancyGrid>("map", 1); ROS_INFO("nh.subscribe, wheel_encoder\n"); encoderSub = nh.subscribe("wheel_encoder", 1, &WheelMap::encoderCallback, this); //ros::spin(); // Allocate the map mapData = new int8_t[AM_MAP_WIDTH*AM_MAP_HEIGHT]; memset(mapData, AM_LM_FREE_SPACE, sizeof(mapData)); // Setup the header & info statically... theHeader.frame_id = "map2"; theInfo.resolution = 0.01; theInfo.width = AM_MAP_WIDTH; theInfo.height = AM_MAP_HEIGHT; theInfo.origin.position.x = -10.0; theInfo.origin.position.y = -10.0; theInfo.origin.position.z = 0.0; // Parameters ros::NodeHandle n_private("~"); double def_offset = 0.0; n_private.param("offset", offset, def_offset); ROS_INFO("Offset: %f", offset); double def_scaleFactor = 15.0; n_private.param("scale_factor", scaleFactor, def_scaleFactor); ROS_INFO("Scale factor: %f", scaleFactor); }
Experiment::Experiment(ros::NodeHandle n):n_(n) { ros::NodeHandle n_private("~"); n_private.param("num_reps", num_reps_, 100); n_private.param("freq", freq_, 2.0); n_private.param("goal_radius", goal_radius_, 0.5); n_private.param("start_radius", start_radius_, 5.0); n_private.param("goalx", goalx_, 0.0); n_private.param("goaly", goaly_, 0.0); n_private.param("bounds/tlx", bounds_[0], -8.0); n_private.param("bounds/tly", bounds_[1], 8.0); n_private.param("bounds/brx", bounds_[2], 8.0); n_private.param("bounds/bry", bounds_[3], -8.0); if (n_private.hasParam("qarray")) learn_ = false; else learn_ = true; srand ( time(NULL) ); cnt_rep_ = 0; cnt_timesteps_ = 0; mode_ = MODE_REP_START; states_ = new States(n); actions_ = new Actions(n); qobj_ = new QLearner(n); //lc_ = new LearningCurve(); path_msg_.header.frame_id = "odom"; move_pub_ = n_.advertise<geometry_msgs::Pose>("move_cmd", 1); path_pub_ = n_.advertise<nav_msgs::Path>("path", 1); path_final_pub_ = n_.advertise<nav_msgs::Path>("path_final", 1); lc_pub_ = n_.advertise<std_msgs::Float64>("learning_times", 1); bool_sub_ = n_.subscribe("move_done", 1, &Experiment::bool_cb, this); odom_sub_ = n.subscribe("base_pose_ground_truth", 10, &Experiment::odom_cb, this); timer_ = n.createTimer(ros::Duration(1/freq_), &Experiment::timer_cb, this); }
IMUPosition::IMUPosition(const ros::NodeHandle& nodeh) { // Init attributes nh = nodeh; // Setup some ROS stuff odomSub = nh.subscribe("odom", 1, &IMUPosition::odomCallback, this); joySub = nh.subscribe("nano2", 1, &IMUPosition::joyCallback, this); imuSub = nh.subscribe("imu", 1, &IMUPosition::imuCallback, this); imuResetSub = nh.subscribe("imu_reset", 1, &IMUPosition::imuResetCallback, this); posePub = nh.advertise<geometry_msgs::PoseStamped>("pose_combined", 10); poseArrayPub = nh.advertise<geometry_msgs::PoseArray>("estimated_poses", 1); helperPub = nh.advertise<visualization_msgs::Marker>("helpers", 25); // Initialize the intial pose robotPose.pose.position.x = 0.0; robotPose.pose.position.y = 0.0; robotPose.pose.position.z = 0.0; xpos = 0.0; ypos = 0.0; zpos = 0.0; heading = 0.0; qyaw = tf::createQuaternionFromYaw(heading); dxTot = 0; dyTot = 0; dhTot = 0; lastOdoHeading = 0; newOdo = false; lastHeading = 0; gotFirst = false; // Parameters ros::NodeHandle n_private("~"); posTopicName = "odom_combined"; n_private.param<std::string>("posTopicName", posTopicName, "odom_combined"); ROS_INFO("Param: posTopicName: %s", posTopicName.c_str()); odomPub = nh.advertise<nav_msgs::Odometry>(posTopicName, 25); // Only use odo? onlyOdo = 1; n_private.param("onlyOdo", onlyOdo, 1); ROS_INFO("Param: onlyOdo: [%d]", onlyOdo); waitingForRelease = false; joyDisabled = true; // Default odom.pose.pose.position.x = 0.0; odom.pose.pose.position.y = 0.0; odom.pose.pose.position.z = 0.0; tf::Quaternion qyaw = tf::createQuaternionFromYaw(0.0); odom.pose.pose.orientation.x = qyaw.x(); odom.pose.pose.orientation.y = qyaw.y(); odom.pose.pose.orientation.z = qyaw.z(); odom.pose.pose.orientation.w = qyaw.w(); odom.twist.twist.linear.x = 0.0; odom.twist.twist.linear.y = 0.0; odom.twist.twist.angular.z = 0.0; setDefaultValues(); smoothHeadingMode = false; imuOffsetYaw = 0.0; imuDeltaYaw = 0.0; lastImuYaw = 0.0; newImu = false; imuOffsetYaw = 0.0; lastTwistZ = 0.0; }
int main(int argc, char **argv) { ros::init(argc, argv, "uvc_cam"); ros::NodeHandle n; ros::NodeHandle n_private("~"); std::string device; std::string out_topic; n_private.param<std::string>("device", device, "/dev/video0"); n_private.param<std::string>("topic", out_topic, "/camera/image"); int width, height, fps, modetect_lum, modetect_count; n_private.param("width", width, 640); n_private.param("height", height, 480); n_private.param("fps", fps, 30); n_private.param("motion_threshold_luminance", modetect_lum, 100); n_private.param("motion_threshold_count", modetect_count, -1); ros::Publisher pub = n.advertise<sensor_msgs::Image>(out_topic.c_str(), 1); ROS_INFO("opening uvc_cam at %dx%d, %d fps", width, height, fps); uvc_cam::Cam cam(device.c_str(), uvc_cam::Cam::MODE_RGB, width, height, fps); cam.set_motion_thresholds(modetect_lum, modetect_count); ros::Time t_prev(ros::Time::now()); int count = 0, skip_count = 0; while (n.ok()) { unsigned char *frame = NULL; uint32_t bytes_used; int buf_idx = cam.grab(&frame, bytes_used); if (count++ % fps == 0) { ros::Time t(ros::Time::now()); ros::Duration d(t - t_prev); ROS_INFO("%.1f fps skip %d", (double)fps / d.toSec(), skip_count); t_prev = t; } if (frame) { sensor_msgs::Image image; image.header.stamp = ros::Time::now(); image.encoding = sensor_msgs::image_encodings::RGB8; image.height = height; image.width = width; image.step = 3 * width; image.set_data_size( image.step * image.height ); /* uint8_t* bgr = &(image.data[0]); for (uint32_t y = 0; y < height; y++) for (uint32_t x = 0; x < width; x++) { // hack... flip bgr to rgb uint8_t *p = frame + y * width * 3 + x * 3; uint8_t *q = bgr + y * width * 3 + x * 3; q[0] = p[2]; q[1] = p[1]; q[2] = p[0]; } */ memcpy(&image.data[0], frame, width * height * 3); pub.publish(image); cam.release(buf_idx); } else skip_count++; } return 0; }
RangeSteering::RangeSteering(const ros::NodeHandle& nodeh) { // Init attributes nh = nodeh; // Parameters ros::NodeHandle n_private("~"); // Setup some ROS stuff joySub = nh.subscribe("nano2", 1, &RangeSteering::joyCallback, this); rangeSub = nh.subscribe("uwb", 1, &RangeSteering::rangeCallback, this); odomSub = nh.subscribe("odom", 1, &RangeSteering::odomCallback, this); odomCombinedSub = nh.subscribe("odom_combined", 1, &RangeSteering::odomCombinedCallback, this); //~ beaconPosSub = nh.subscribe("beacon_pos", 1, &RangeSteering::beaconPosCallback, this); beaconPub = nh.advertise<visualization_msgs::Marker>("helpers", 1); cmdPub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1); modePub = nh.advertise<std_msgs::UInt16>("cmd_mode", 1); statusSub = nh.subscribe("sensor_status", 1, &RangeSteering::statusCallback, this); coveragePub = nh.advertise<nav_msgs::Path>("coverage_plan", 1); // Special simulation noise std::string defRobotId = "DECA0100-100"; n_private.param("robotId", robotId, defRobotId); ROS_INFO("Param: robotId: [%s]", robotId.c_str()); addBeacons = true; // Left beaconList[0].id = "DECA0100-101"; beaconList[0].xpos = 0.0; beaconList[0].ypos = 0.50; beaconList[0].zpos = 0.0; beaconList[0].heading = 0.0; beaconList[0].range = 0.0; // Rigth beaconList[1].id = "DECA0100-102"; beaconList[1].xpos = 0.0; beaconList[1].ypos = -0.50; beaconList[1].zpos = 0.0; beaconList[1].heading = 0.0; beaconList[1].range = 0.0; /* Local positioning stuff*/ bearingEst = 0; minRange = 10000; dockStart = 1.0; /* When to enter local docking mode */ newRange = false; doControl = false; radius = 1.0; radiusInc = 0.15; speed = 1.0; turnSpeed = 1.0; leftRange = 0; rightRange = 0; waitingForRelease = false; joyDisabled = true; dockStarted = false; seekDockStart = false; followDirection = 1; kpDock = 1.0; kdDock = 9.6; kiDock = 0.00; kp = 0.7; kd = 7.25; ki = 0.20; numInterBeaconSamples = 0; /* How much before the baseline between home beacons shall it stop? */ baselineOffset = 0.5; rangeHome = 0; errOld = 0.0; passed180 = false; state = STATE_START_FOLLOW_RADIUS; rangeState = STATE_DETECT_BEACONS; }
int main(int argc, char **argv) { // Initialize ROS ros::init(argc, argv, "am_mpu9150_node"); ros::NodeHandle n; ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu", 1); ros::Publisher imu_euler_pub = n.advertise<geometry_msgs::Vector3Stamped>("imu_euler", 1); ros::Rate loop_rate(50); sensor_msgs::Imu imu; imu.header.frame_id = "imu"; geometry_msgs::Vector3Stamped euler; euler.header.frame_id = "imu_euler"; // Init the sensor the values are hardcoded at the local_defaults.h file int opt, len; int i2c_bus = DEFAULT_I2C_BUS; int sample_rate = DEFAULT_SAMPLE_RATE_HZ; int yaw_mix_factor = DEFAULT_YAW_MIX_FACTOR; int verbose = 0; std::string accelFile; std::string magFile; // Parameters from ROS ros::NodeHandle n_private("~"); int def_i2c_bus = 2; n_private.param("i2cbus", i2c_bus, def_i2c_bus); if (i2c_bus < MIN_I2C_BUS || i2c_bus > MAX_I2C_BUS) { ROS_ERROR("am_mpu9150: Imu Bus problem"); return -1; } int def_sample_rate = 50; n_private.param("samplerate", sample_rate, def_sample_rate); if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) { ROS_ERROR("am_mpu9150: Sample rate problem"); return -1; } loop_rate = sample_rate; int def_yaw_mix_factor = 0; n_private.param("yawmixfactor", yaw_mix_factor, def_yaw_mix_factor); if (yaw_mix_factor < 0 || yaw_mix_factor > 100) { ROS_ERROR("am_mpu9150: yawmixfactor problem"); return -1; } std::string defAccelFile = "./accelcal.txt"; n_private.param("accelfile", accelFile, defAccelFile); std::string defMagFile = "./magcal.txt"; n_private.param("magfile", magFile, defMagFile); unsigned long loop_delay; mpudata_t mpu; // Initialize the MPU-9150 mpu9150_set_debug(verbose); if (mpu9150_init(i2c_bus, sample_rate, yaw_mix_factor)) { ROS_ERROR("am_mpu9150::Failed init!"); exit(1); } //load_calibration(0, accelFile); //load_calibration(1, magFile); memset(&mpu, 0, sizeof(mpudata_t)); vector3d_t e; quaternion_t q; while (ros::ok()) { std::stringstream ss; if (mpu9150_read(&mpu) == 0) { // ROTATE The angles correctly... quaternionToEuler(mpu.fusedQuat, e); e[VEC3_Z] = -e[VEC3_Z]; eulerToQuaternion(e, q); // FUSED imu.header.stamp = ros::Time::now(); imu.orientation.x = q[QUAT_X]; imu.orientation.y = q[QUAT_Y]; imu.orientation.z = q[QUAT_Z]; imu.orientation.w = q[QUAT_W]; // GYRO double gx = mpu.rawGyro[VEC3_X]; double gy = mpu.rawGyro[VEC3_Y]; double gz = mpu.rawGyro[VEC3_Z]; gx = gx * gyroScaleX; gy = gy * gyroScaleY; gz = gz * gyroScaleZ; imu.angular_velocity.x = gx; imu.angular_velocity.y = gy; imu.angular_velocity.z = gz; // ACCEL imu.linear_acceleration.x = mpu.calibratedAccel[VEC3_X]; imu.linear_acceleration.y = mpu.calibratedAccel[VEC3_Y]; imu.linear_acceleration.z = mpu.calibratedAccel[VEC3_Z]; // EULER euler.header.stamp = imu.header.stamp; euler.vector.x = mpu.fusedEuler[VEC3_Y]; euler.vector.y = mpu.fusedEuler[VEC3_X]; euler.vector.z = -mpu.fusedEuler[VEC3_Z]; // Publish the messages imu_pub.publish(imu); imu_euler_pub.publish(euler); //ROS_INFO("y=%f, p=%f, r=%f", euler.vector.z, euler.vector.y, euler.vector.x); } ros::spinOnce(); loop_rate.sleep(); } return 0; }
TP8::TP8(const double map_resolution, const double map_length, const double map_border, const uint delta_save, const double max_lin_vel, const double max_ang_vel, std::string robotname): _MAP_RESOLUTION(map_resolution), _MAP_LENGTH(map_length), _MAP_BORDER(map_border), _DELTA_SAVE(delta_save), _lin_vel(0.0), _ang_vel(0.0), _avoid(false), _rotating(false), _rotate_left(false), _MAX_LIN_VEL(max_lin_vel), _MAX_ANG_VEL(max_ang_vel), _odom_updated(false), _odom_first_update(true), _iteration(0) { // Specify markers positions _markers_wpos[0].x = -8.0; _markers_wpos[0].y = 4.0; _markers_wpos[1].x = -8.0; _markers_wpos[1].y = -4.0; _markers_wpos[2].x = -3.0; _markers_wpos[2].y = -8.0; _markers_wpos[3].x = 3.0; _markers_wpos[3].y = -8.0; _markers_wpos[4].x = 8.0; _markers_wpos[4].y = -4.0; _markers_wpos[5].x = 8.0; _markers_wpos[5].y = 4.0; _markers_wpos[6].x = 3.0; _markers_wpos[6].y = 8.0; _markers_wpos[7].x = -3.0; _markers_wpos[7].y = 8.0; // Open file to store robot poses _outfile.open("Output.txt"); // Write file header _outfile << "Estimated and real pose of the robot\n\n" << "[T]: Odometry [X Y Theta] Real [X Y Theta] Particle [X Y Theta]\n\n"; // // Update/create map related variables // // We will create an image with the map wich contains the original map plus // a border around it, so as to allow estimates outside of the original // map. // Read original map and resize it to our resolution std::string map_file_path(getenv("HOME")); map_file_path += "/ros/worlds/stage/cave.png"; cv::Mat org_full_size_map = cv::imread(map_file_path, CV_LOAD_IMAGE_COLOR); if( org_full_size_map.data == 0 ) std::cerr << "Error reading map, program will shutdown!" << std::endl; _org_map.create(ceil(_MAP_LENGTH/_MAP_RESOLUTION), ceil(_MAP_LENGTH/_MAP_RESOLUTION), CV_8UC3); cv::resize(org_full_size_map, _org_map, cv::Size(ceil(_MAP_LENGTH/_MAP_RESOLUTION), ceil(_MAP_LENGTH/_MAP_RESOLUTION)), 0, 0, cv::INTER_AREA ); // This will be our actual map, with the border around it. _map.create(ceil((_MAP_LENGTH+_MAP_BORDER)/_MAP_RESOLUTION), ceil((_MAP_LENGTH+_MAP_BORDER)/_MAP_RESOLUTION), CV_8UC3); cv::copyMakeBorder(_org_map, _map, floor((_map.size().height-_org_map.size().height)/2), ceil((_map.size().height-_org_map.size().height)/2), floor((_map.size().width-_org_map.size().width)/2), ceil((_map.size().width-_org_map.size().width)/2), cv::BORDER_CONSTANT, cv::Scalar(255,255,255)); // Create window for the map cv::namedWindow("Debug", 0); // Initialize random number generator _rng(time(NULL)); /// Initilize Kalman filter related variables // The state holds X, Y and Theta estimate of the robot, all of type double. _ekf._state.create(3, 1); // Covariance matrix associated with the robot movement _ekf._V = (cv::Mat_<double>(3,3) << 0.10*0.10, 0.00, 0.00, 0.00, 0.03*0.03, 0.00, 0.00, 0.00, 0.20*0.20); // Process dynamics jacobian regarding the state // Only elements (1,3) and (2,3) need to be updated, so we set the correct //values for the other ones now. _ekf._Fx = (cv::Mat_<double>(3,3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); // Process dynamics jacobian regarding the inputs // Only elements (0,0), (0,1), (1,0) and (1,1) need to be updated, so we set // the correct the other ones now. _ekf._Fv = (cv::Mat_<double>(3,3) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); // Process covariance (confidence associated with current state) // (It is called Σ in the theoretic material) _ekf._P = cv::Mat::zeros(3, 3, CV_64F); // Get parameters ros::NodeHandle n_private("~"); n_private.param("min_front_dist", _min_front_dist, 1.0); n_private.param("stop_front_dist", _stop_front_dist, 0.6); std::cout << "Random navigation with obstacle avoidance and EKF-based" << " localization\n---------------------------" << std::endl; /// Setup subscribers _sub_real_pose = _nh.subscribe(robotname + "/base_pose_ground_truth", 1, &TP8::realPoseCallback, this); _sub_odom = _nh.subscribe(robotname + "/odom", 10, &TP8::odomCallback, this); _sub_laser = _nh.subscribe(robotname + "/base_scan", 1, &TP8::laserCallback, this); _sub_markers = _nh.subscribe(robotname + "/markers", 1, &TP8::markersCallback, this); /// Setup publisher for linear and angular velocity _vel_pub = _nh.advertise<geometry_msgs::Twist>(robotname + "/cmd_vel", 1); /// Wait until we get an initial robot pose (from odometry) ros::Rate cycle(10.0); // Cycle rate while( _odom_updated == false ) { ros::spinOnce(); cycle.sleep(); } /// Stop the robot (if not stopped already) _vel_cmd.linear.x = _lin_vel; _vel_cmd.angular.z = _ang_vel; _vel_pub.publish(_vel_cmd); }
/** * Main function * Navigate randomly using distance sensor data and Particle Filter-based * localization. */ int main(int argc, char** argv) { std::string robot_name = "/robot_0"; // Open file to store robot poses outfile.open("Output.txt"); // Write file header outfile << "Estimated and real pose of the robot\n\n" << "[T]: Odometry [X Y Theta] Real [X Y Theta] Particle [X Y Theta]\n\n"; // // Create map related variables // // We will create an image with the map wich contains the original map plus // a border around it, so as to allow estimates outside of the original // map. std::string map_file_path(getenv("HOME")); map_file_path += "/ros/worlds/stage/cave.png"; cv::Mat cave_map = cv::imread(map_file_path, CV_LOAD_IMAGE_COLOR); if( cave_map.data == 0 ) { std::cerr << "Error reading map!" << std::endl; return -1; } // Read original map and resize it to our resolution cv::resize(cave_map, map, map.size(), 0, 0, cv::INTER_AREA ); // Read original map and resize it to our resolution // We need the original map, black and white, so that it is faster to find // occupied cells cv::resize(cv::imread(map_file_path, CV_LOAD_IMAGE_GRAYSCALE), org_map, org_map.size(), 0, 0, cv::INTER_AREA ); // We need a temporary map tmp_map = map.clone(); // Create window for the map cv::namedWindow("Debug", 0); // // Particle filter related variables with their initial values // // This variable will hold our final estimation at each iteration geometry_msgs::Pose2D pose_estimate; // Initialize particles with uniform random distribution across all space cv::randu( particles_x, -MAP_LENGTH/2.0+SAFETY_BORDER, MAP_LENGTH/2.0-SAFETY_BORDER); // X cv::randu( particles_y, -MAP_LENGTH/2.0+SAFETY_BORDER, MAP_LENGTH/2.0-SAFETY_BORDER); // Y cv::randu( particles_theta, -CV_PI, CV_PI ); // Theta // Re-add particles that are outside of the map or inside an obstacle, until // it is in a free space inside the map. for(int n = 0; n < NUM_PARTICLES; n++) { while( ( (particles_x(n,0) > MAP_LENGTH/2.0) || (particles_x(n,0) < -MAP_LENGTH/2.0) || (particles_y(n,0) > MAP_LENGTH/2.0) || (particles_y(n,0) < -MAP_LENGTH/2.0) ) || (org_map.at<uchar>( cvRound(org_map.size().height/2.0 - particles_y(n,0)/MAP_RESOLUTION), cvRound(org_map.size().width/2.0 + particles_x(n,0)/MAP_RESOLUTION)) < 127) ) { particles_x(n,0) = rng.uniform(-MAP_LENGTH/2.0+SAFETY_BORDER, MAP_LENGTH/2.0-SAFETY_BORDER); particles_y(n,0) = rng.uniform(-MAP_LENGTH/2.0+SAFETY_BORDER, MAP_LENGTH/2.0-SAFETY_BORDER); } } // Init ROS ros::init(argc, argv, "tp7"); // ROS variables/objects ros::NodeHandle nh; // Node handle // Get parameters ros::NodeHandle n_private("~"); n_private.param("min_front_dist", min_front_dist, 1.0); n_private.param("stop_front_dist", stop_front_dist, 0.6); std::cout << "Random navigation with obstacle avoidance and particle filter" << " based localization\n---------------------------" << std::endl; /// Setup subscribers // Odometry ros::Subscriber sub_odom = nh.subscribe(robot_name + "/odom", 10, odomCallback); // Real, error-free robot pose (for debug purposes only) ros::Subscriber sub_real_pose = nh.subscribe(robot_name + "/base_pose_ground_truth", 1, realPoseCallback); // Laser scans ros::Subscriber sub_laser = nh.subscribe(robot_name + "/base_scan", 1, laserCallback); // Markers detected ros::Subscriber sub_markers = nh.subscribe(robot_name + "/markers", 1, markersCallback); // Setup publisher for linear and angular velocity vel_pub = nh.advertise<geometry_msgs::Twist>(robot_name + "/cmd_vel", 1); ros::Rate cycle(10.0); // Cycle rate // Wait until we get the robot pose (from odometry) while( odom_updated == false ) { ros::spinOnce(); cycle.sleep(); } // Stop the robot (if not stopped already) vel_cmd.angular.z = 0; vel_cmd.linear.x = 0; vel_pub.publish(vel_cmd); // Infinite loop (will call the callbacks whenever information is available, // until ros::shutdown() is called. ros::spin(); // If we are quitting, stop the robot vel_cmd.angular.z = 0; vel_cmd.linear.x = 0; vel_pub.publish(vel_cmd); // Close file outfile.close(); // Store final map cv::imwrite("mapa.png", map); return 1; }
/** * Main function * Controls the robot using the keyboard keys and outputs posture and velocity * related information. */ int main(int argc, char** argv) { // Open file to store robot poses outfile.open("Postures.txt"); // Create a window to show the map cv::namedWindow("Mapa", 0); // // Create map related variables // map.create(ceil(MAP_HEIGHT/MAP_RESOLUTION), ceil(MAP_WIDTH/MAP_RESOLUTION), CV_8UC1); // Set the initial map cell values to "undefined" map.setTo(127); // // Create robot related objects // // Linear and angular velocities for the robot (initially stopped) double lin_vel=0, ang_vel=0; double last_ang_vel = DEG2RAD(15); // Navigation variables bool avoid, new_rotation = false; double stop_front_dist, min_front_dist; // Random number generator (used for direction choice) CvRNG rnggstate = cvRNG(0xffffffff); // Init ROS ros::init(argc, argv, "tp4"); // ROS variables/objects ros::NodeHandle nh; // Node handle ros::Publisher vel_pub; // Velocity commands publisher geometry_msgs::Twist vel_cmd; // Velocity commands std::cout << "Random navigation with obstacle avoidance and map generation\n" << "---------------------------" << std::endl; // Get parameters ros::NodeHandle n_private("~"); n_private.param("min_front_dist", min_front_dist, 1.0); n_private.param("stop_front_dist", stop_front_dist, 0.6); /// Setup subscribers // Odometry ros::Subscriber sub_odom = nh.subscribe("/robot_0/odom", 1, odomCallback); // Laser scans ros::Subscriber sub_laser = nh.subscribe("/robot_0/base_scan", 1, laserCallback); // Point clouds ros::Subscriber sub_pcl = nh.subscribe("cloud_filtered", 1, pointCloudCallback); /// Setup publisher vel_pub = nh.advertise<geometry_msgs::Twist>("/robot_0/cmd_vel", 1); // Infinite loop ros::Rate cycle(10.0); // Rate when no key is being pressed while(ros::ok()) { // Get data from the robot and print it if available ros::spinOnce(); // Only change navigation controls if laser was updated if( laser_updated == false ) continue; // show pose estimated from odometry std::cout << std::setiosflags(std::ios::fixed) << std::setprecision(3) << "Robot estimated pose = " << robot_pose.x << " [m], " << robot_pose.y << " [m], " << RAD2DEG(robot_pose.theta) << " [º]\n"; // Show estimated velocity std::cout << "Robot estimated velocity = " << true_lin_vel << " [m/s], " << RAD2DEG(true_ang_vel) << " [º/s]\n"; // Check for obstacles near the front of the robot avoid = false; if( closest_front_obstacle < min_front_dist ) { if( closest_front_obstacle < stop_front_dist ) { avoid = true; lin_vel = -0.100; } else { avoid = true; lin_vel = 0; } } else { lin_vel = 0.8; ang_vel = 0; new_rotation = false; } // Rotate to avoid obstacles if(avoid) { if( new_rotation == false ) { float rnd_point = cvRandReal(&rnggstate ); if( rnd_point >= 0.9 ) { last_ang_vel = -last_ang_vel; } } ang_vel = last_ang_vel; new_rotation = true; } // Limit maximum velocities // (not needed here) // lin_vel = clipValue(lin_vel, -MAX_LIN_VEL, MAX_LIN_VEL); // ang_vel = clipValue(ang_vel, -MAX_ANG_VEL, MAX_ANG_VEL); // Show desired velocity std::cout << "Robot desired velocity = " << lin_vel << " [m/s], " << RAD2DEG(lin_vel) << " [º/s]" << std::endl; // Send velocity commands vel_cmd.angular.z = ang_vel; vel_cmd.linear.x = lin_vel; vel_pub.publish(vel_cmd); // Terminate loop if Escape key is pressed if( cv::waitKey(10) == 27 ) break; // Proceed at desired framerate cycle.sleep(); } // If we are quitting, stop the robot vel_cmd.angular.z = 0; vel_cmd.linear.x = 0; vel_pub.publish(vel_cmd); // Store map cv::imwrite("mapa.png", map); // Close file outfile.close(); return 1; }