示例#1
0
//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);


}
示例#2
0
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("~");

        }
示例#4
0
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);
}
示例#6
0
    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);
        }
示例#10
0
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]);
		}
  }
示例#11
0
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);   
 }
示例#14
0
 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);
 }
示例#15
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("~");
  }
示例#18
0
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);
}
示例#19
0
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;
}
示例#20
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;

  }
示例#22
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);
}
示例#24
0
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;
}
示例#25
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;
}
示例#26
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;
}
示例#27
0
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;
}
示例#28
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);
}
示例#29
0
/**
 * 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;
}
示例#30
0
/**
 * 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;
}