示例#1
0
int main(int argc, char **argv)
{
    ros::init (argc, argv, "workspace_analysis");
    ros::AsyncSpinner spinner(1);
    spinner.start();

    // wait some time for everything to be loaded correctly...
    ROS_INFO_STREAM("Waiting a few seconds to load the robot description correctly...");
    sleep(3);
    ROS_INFO_STREAM("Hope this was enough!");

    /*Get some ROS params */
    ros::NodeHandle node_handle("~");
    double res_x, res_y, res_z;
    double min_x, min_y, min_z;
    double max_x, max_y, max_z;

    double roll, pitch, yaw;
    int max_attempts;
    double joint_limits_penalty_multiplier;
    std::string group_name;

    if (!node_handle.getParam("min_x", min_x))
        min_x = 0.0;
    if (!node_handle.getParam("max_x", max_x))
        max_x = 0.0;
    if (!node_handle.getParam("res_x", res_x))
        res_x = 0.1;

    if (!node_handle.getParam("min_y", min_y))
        min_y = 0.0;
    if (!node_handle.getParam("max_y", max_y))
        max_y = 0.0;
    if (!node_handle.getParam("res_y", res_y))
        res_y = 0.1;

    if (!node_handle.getParam("min_z", min_z))
        min_z = 0.0;
    if (!node_handle.getParam("max_z", max_z))
        max_z = 0.0;
    if (!node_handle.getParam("res_z", res_z))
        res_z = 0.1;
    if (!node_handle.getParam("max_attempts", max_attempts))
        max_attempts = 10000;

    if (!node_handle.getParam("roll", roll))
        roll = 0.0;
    if (!node_handle.getParam("pitch", pitch))
        pitch = 0.0;
    if (!node_handle.getParam("yaw", yaw))
        yaw = 0.0;

    if (!node_handle.getParam("joint_limits_penalty_multiplier", joint_limits_penalty_multiplier))
        joint_limits_penalty_multiplier = 0.0;

    std::string filename;
    if (!node_handle.getParam("filename", filename))
        ROS_ERROR("Will not write to file");

    std::string quat_filename;
    if (!node_handle.getParam("quat_filename", quat_filename))
        ROS_ERROR("Will not write to file");

    if (!node_handle.getParam("group_name", group_name))
        ROS_FATAL("Must have valid group name");

    /* Load the robot model */
    robot_model_loader::RobotModelLoader robot_model_loader("robot_description");

    /* Get a shared pointer to the model */
    robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();

    /* Create a robot state*/
    robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));

    if(!robot_model->hasJointModelGroup(group_name))
        ROS_FATAL("Invalid group name: %s", group_name.c_str());

    const robot_model::JointModelGroup* joint_model_group = robot_model->getJointModelGroup(group_name);

    /* Construct a planning scene - NOTE: this is for illustration purposes only.
       The recommended way to construct a planning scene is to use the planning_scene_monitor
       to construct it for you.*/
    planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));

    moveit_msgs::WorkspaceParameters workspace;
    workspace.min_corner.x = min_x;
    workspace.min_corner.y = min_y;
    workspace.min_corner.z = min_z;

    workspace.max_corner.x = max_x;
    workspace.max_corner.y = max_y;
    workspace.max_corner.z = max_z;

    /* Load the workspace analysis */
    moveit_workspace_analysis::WorkspaceAnalysis workspace_analysis(planning_scene, true, joint_limits_penalty_multiplier);

    ros::Time init_time;
    moveit_workspace_analysis::WorkspaceMetrics metrics;

    /* Compute the metrics */

    bool use_vigir_rpy = true;
    if (use_vigir_rpy) {
        std::vector<geometry_msgs::Quaternion> orientations;
        geometry_msgs::Quaternion quaternion;

        //yaw = -M_PI*0.0;
        //roll = M_PI*0.49;

        // translate roll, pitch and yaw into a Quaternion
        tf::Quaternion q;
        q.setRPY(roll, pitch, yaw);
        geometry_msgs::Quaternion odom_quat;
        tf::quaternionTFToMsg(q, quaternion);

        orientations.push_back(quaternion);

        metrics = workspace_analysis.computeMetrics(workspace, orientations, robot_state.get(), joint_model_group, res_x, res_y, res_z);
    } else {

        // load the set of quaternions
        std::vector<geometry_msgs::Quaternion> orientations;
        std::ifstream quat_file;
        quat_file.open(quat_filename.c_str());
        while(!quat_file.eof())
        {
            geometry_msgs::Quaternion temp_quat;
            orientations.push_back(temp_quat);
        }

        init_time = ros::Time::now();

        metrics = workspace_analysis.computeMetrics(workspace, orientations, robot_state.get(), joint_model_group, res_x, res_y, res_z);

        if(metrics.points_.empty())
            ROS_WARN_STREAM("No point to be written to file: consider changing the workspace, or recompiling moveit_workspace_analysis with a longer sleeping time at the beginning (if this could be the cause)");
    }

    //ros::WallDuration duration(100.0);
    //moveit_workspace_analysis::WorkspaceMetrics metrics = workspace_analysis.computeMetricsFK(&(*robot_state), joint_model_group, max_attempts, duration);

    if(!filename.empty())
        if(!metrics.writeToFile(filename,",",false))
            ROS_INFO("Could not write to file");

    ros::Duration total_duration = ros::Time::now() - init_time;
    ROS_INFO_STREAM("Total duration: " << total_duration.toSec() << "s");

    ros::shutdown();
    return 0;
}
int main(int argc, char** argv)
{
  ros::init(argc, argv, "exploration");
  ros::NodeHandle nh;
  ros::Publisher trajectory_pub = nh.advertise < trajectory_msgs::MultiDOFJointTrajectory
      > (mav_msgs::default_topics::COMMAND_TRAJECTORY, 5);
  ROS_INFO("Started exploration");

  std_srvs::Empty srv;
  bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);
  unsigned int i = 0;

  // Trying to unpause Gazebo for 10 seconds.
  while (i <= 10 && !unpaused) {
    ROS_INFO("Wait for 1 second before trying to unpause Gazebo again.");
    std::this_thread::sleep_for(std::chrono::seconds(1));
    unpaused = ros::service::call("/gazebo/unpause_physics", srv);
    ++i;
  }

  if (!unpaused) {
    ROS_FATAL("Could not wake up Gazebo.");
    return -1;
  } else {
    ROS_INFO("Unpaused the Gazebo simulation.");
  }

  double dt = 1.0;
  std::string ns = ros::this_node::getName();
  if (!ros::param::get(ns + "/nbvp/dt", dt)) {
    ROS_FATAL("Could not start exploration. Parameter missing! Looking for %s",
              (ns + "/nbvp/dt").c_str());
    return -1;
  }

  static int n_seq = 0;

  trajectory_msgs::MultiDOFJointTrajectory samples_array;
  mav_msgs::EigenTrajectoryPoint trajectory_point;
  trajectory_msgs::MultiDOFJointTrajectoryPoint trajectory_point_msg;

  // Wait for 5 seconds to let the Gazebo GUI show up.
  ros::Duration(5.0).sleep();

  // This is the initialization motion, necessary that the known free space allows the planning
  // of initial paths.
  ROS_INFO("Starting the planner: Performing initialization motion");
  for (double i = 0; i <= 1.0; i = i + 0.1) {
    nh.param<double>("wp_x", trajectory_point.position_W.x(), 0.0);
    nh.param<double>("wp_y", trajectory_point.position_W.y(), 0.0);
    nh.param<double>("wp_z", trajectory_point.position_W.z(), 1.0);
    samples_array.header.seq = n_seq;
    samples_array.header.stamp = ros::Time::now();
    samples_array.points.clear();
    n_seq++;
    tf::Quaternion quat = tf::Quaternion(tf::Vector3(0.0, 0.0, 1.0), -M_PI * i);
    trajectory_point.setFromYaw(tf::getYaw(quat));
    mav_msgs::msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &trajectory_point_msg);
    samples_array.points.push_back(trajectory_point_msg);
    trajectory_pub.publish(samples_array);
    ros::Duration(1.0).sleep();
  }
  trajectory_point.position_W.x() -= 0.5;
  trajectory_point.position_W.y() -= 0.5;
  samples_array.header.seq = n_seq;
  samples_array.header.stamp = ros::Time::now();
  samples_array.points.clear();
  n_seq++;
  mav_msgs::msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &trajectory_point_msg);
  samples_array.points.push_back(trajectory_point_msg);
  trajectory_pub.publish(samples_array);
  ros::Duration(1.0).sleep();

  // Start planning: The planner is called and the computed path sent to the controller.
  int iteration = 0;
  while (ros::ok()) {
    ROS_INFO_THROTTLE(0.5, "Planning iteration %i", iteration);
    nbvplanner::nbvp_srv planSrv;
    planSrv.request.header.stamp = ros::Time::now();
    planSrv.request.header.seq = iteration;
    planSrv.request.header.frame_id = "world";
    if (ros::service::call("nbvplanner", planSrv)) {
      n_seq++;
      if (planSrv.response.path.size() == 0) {
        ros::Duration(1.0).sleep();
      }
      for (int i = 0; i < planSrv.response.path.size(); i++) {
        samples_array.header.seq = n_seq;
        samples_array.header.stamp = ros::Time::now();
        samples_array.header.frame_id = "world";
        samples_array.points.clear();
        tf::Pose pose;
        tf::poseMsgToTF(planSrv.response.path[i], pose);
        double yaw = tf::getYaw(pose.getRotation());
        trajectory_point.position_W.x() = planSrv.response.path[i].position.x;
        trajectory_point.position_W.y() = planSrv.response.path[i].position.y;
        // Add offset to account for constant tracking error of controller
        trajectory_point.position_W.z() = planSrv.response.path[i].position.z + 0.25;
        tf::Quaternion quat = tf::Quaternion(tf::Vector3(0.0, 0.0, 1.0), yaw);
        trajectory_point.setFromYaw(tf::getYaw(quat));
        mav_msgs::msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &trajectory_point_msg);
        samples_array.points.push_back(trajectory_point_msg);
        trajectory_pub.publish(samples_array);
        ros::Duration(dt).sleep();
      }
    } else {
      ROS_WARN_THROTTLE(1, "Planner not reachable");
      ros::Duration(1.0).sleep();
    }
    iteration++;
  }
}
示例#3
0
void FeatureViewer::resetDetector()
{  
  gft_config_server_.reset();
  star_config_server_.reset();
  orb_config_server_.reset();
  surf_config_server_.reset();
  
  if (detector_type_ == "ORB")
  { 
    ROS_INFO("Creating ORB detector");
    feature_detector_.reset(new rgbdtools::OrbDetector());
    orb_config_server_.reset(new 
      OrbDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/ORB")));
    
    // dynamic reconfigure
    OrbDetectorConfigServer::CallbackType f = boost::bind(
      &FeatureViewer::orbReconfigCallback, this, _1, _2);
    orb_config_server_->setCallback(f);
  }
  else if (detector_type_ == "SURF")
  {
    ROS_INFO("Creating SURF detector");
    feature_detector_.reset(new rgbdtools::SurfDetector());
    surf_config_server_.reset(new 
      SurfDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/SURF")));
    
    // dynamic reconfigure
    SurfDetectorConfigServer::CallbackType f = boost::bind(
      &FeatureViewer::surfReconfigCallback, this, _1, _2);
    surf_config_server_->setCallback(f);
  }
  else if (detector_type_ == "GFT")
  {
    ROS_INFO("Creating GFT detector");
    feature_detector_.reset(new rgbdtools::GftDetector());
    gft_config_server_.reset(new 
      GftDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/GFT")));
    
    // dynamic reconfigure
    GftDetectorConfigServer::CallbackType f = boost::bind(
      &FeatureViewer::gftReconfigCallback, this, _1, _2);
    gft_config_server_->setCallback(f);
  }
  else if (detector_type_ == "STAR")
  {
    ROS_INFO("Creating STAR detector");
    feature_detector_.reset(new rgbdtools::StarDetector());
    star_config_server_.reset(new 
      StarDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/STAR")));
    
    // dynamic reconfigure
    StarDetectorConfigServer::CallbackType f = boost::bind(
      &FeatureViewer::starReconfigCallback, this, _1, _2);
    star_config_server_->setCallback(f);
  }
  else
  {
    ROS_FATAL("%s is not a valid detector type! Using GFT", detector_type_.c_str());
    feature_detector_.reset(new rgbdtools::GftDetector());
    gft_config_server_.reset(new 
      GftDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/GFT")));
    
    // dynamic reconfigure
    GftDetectorConfigServer::CallbackType f = boost::bind(
      &FeatureViewer::gftReconfigCallback, this, _1, _2);
    gft_config_server_->setCallback(f);
  }
}
示例#4
0
int main(int argc, char** argv)
{
	ros::init(argc, argv, "roomba560_node");

	ROS_INFO("Roomba for ROS %.2f", NODE_VERSION);
	
	double last_x, last_y, last_yaw;
	double vel_x, vel_y, vel_yaw;
	double dt;
	float last_charge = 0.0;
	int time_remaining = -1;
	
	ros::NodeHandle n;
	ros::NodeHandle pn("~");
	
	pn.param<std::string>("port", port, "/dev/ttyUSB0");
	
	std::string base_frame_id;
	std::string odom_frame_id;
	pn.param<std::string>("base_frame_id", base_frame_id, "base_link");
	pn.param<std::string>("odom_frame_id", odom_frame_id, "odom");
	
	roomba = new irobot::OpenInterface(port.c_str());

	ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("/odom", 50);
    ros::Publisher battery_pub = n.advertise<irobotcreate2::Battery>("/battery", 50);
    ros::Publisher bumper_pub = n.advertise<irobotcreate2::Bumper>("/bumper", 50);
    ros::Publisher buttons_pub = n.advertise<irobotcreate2::Buttons>("/buttons", 50);
    ros::Publisher cliff_pub = n.advertise<irobotcreate2::RoombaIR>("/cliff", 50);
    ros::Publisher irbumper_pub = n.advertise<irobotcreate2::RoombaIR>("/ir_bumper", 50);
    ros::Publisher irchar_pub = n.advertise<irobotcreate2::IRCharacter>("/ir_character", 50);
    ros::Publisher wheeldrop_pub = n.advertise<irobotcreate2::WheelDrop>("/wheel_drop", 50);

	tf::TransformBroadcaster tf_broadcaster;
	
	ros::Subscriber cmd_vel_sub  = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 1, cmdVelReceived);
    ros::Subscriber leds_sub  = n.subscribe<irobotcreate2::Leds>("/leds", 1, ledsReceived);
    ros::Subscriber digitleds_sub  = n.subscribe<irobotcreate2::DigitLeds>("/digit_leds", 1, digitLedsReceived);
    ros::Subscriber song_sub  = n.subscribe<irobotcreate2::Song>("/song", 1, songReceived);
    ros::Subscriber playsong_sub  = n.subscribe<irobotcreate2::PlaySong>("/play_song", 1, playSongReceived);
    ros::Subscriber mode_sub  = n.subscribe<std_msgs::String>("/mode", 1, cmdModeReceived);
	
	irobot::OI_Packet_ID sensor_packets[1] = {irobot::OI_PACKET_GROUP_100};
	roomba->setSensorPackets(sensor_packets, 1, OI_PACKET_GROUP_100_SIZE);

	if( roomba->openSerialPort(true) == 0) ROS_INFO("Connected to Roomba.");
	else
	{
		ROS_FATAL("Could not connect to Roomba.");
		ROS_BREAK();
	}
	
	ros::Time current_time, last_time;
	current_time = ros::Time::now();
	last_time = ros::Time::now();

	bool first_loop=true;

	ros::Rate r(10.0);
	while(n.ok())
	{
		current_time = ros::Time::now();
		
		last_x = roomba->odometry_x_;
		last_y = roomba->odometry_y_;
		last_yaw = roomba->odometry_yaw_;
		
		if( roomba->getSensorPackets(100) == -1) ROS_ERROR("Could not retrieve sensor packets.");
		else roomba->calculateOdometry();
		
		dt = (current_time - last_time).toSec();
		vel_x = (roomba->odometry_x_ - last_x)/dt;
		vel_y = (roomba->odometry_y_ - last_y)/dt;
		vel_yaw = (roomba->odometry_yaw_ - last_yaw)/dt;
		
		// ******************************************************************************************
		//first, we'll publish the transforms over tf
		geometry_msgs::TransformStamped odom_trans;
		odom_trans.header.stamp = current_time;
		odom_trans.header.frame_id = odom_frame_id;
		odom_trans.child_frame_id = base_frame_id;
		odom_trans.transform.translation.x = roomba->odometry_x_;
		odom_trans.transform.translation.y = roomba->odometry_y_;
		odom_trans.transform.translation.z = 0.0;
		odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_);
		tf_broadcaster.sendTransform(odom_trans);
		
		//TODO: Finish brodcasting the tf for all the ir sensors on the Roomba
		/*geometry_msgs::TransformStamped cliff_left_trans;
		cliff_left_trans.header.stamp = current_time;
		cliff_left_trans.header.frame_id = "base_link";
		cliff_left_trans.child_frame_id = "base_cliff_left";
		cliff_left_trans.transform.translation.x = 0.0;
		cliff_left_trans.transform.translation.y = 0.0;
		cliff_left_trans.transform.translation.z = 0.0;
		cliff_left_trans.transform.rotation = ;
		tf_broadcaster.sendTransform(cliff_left_trans);	*/

		// ******************************************************************************************
		//next, we'll publish the odometry message over ROS
		nav_msgs::Odometry odom;
		odom.header.stamp = current_time;
		odom.header.frame_id = odom_frame_id;
		
		//set the position
		odom.pose.pose.position.x = roomba->odometry_x_;
		odom.pose.pose.position.y = roomba->odometry_y_;
		odom.pose.pose.position.z = 0.0;
		odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_);
		
		//set the velocity
		odom.child_frame_id = base_frame_id;
		odom.twist.twist.linear.x = vel_x;
		odom.twist.twist.linear.y = vel_y;
		odom.twist.twist.angular.z = vel_yaw;
		
		//publish the message
		odom_pub.publish(odom);

		// ******************************************************************************************
		//publish battery
        irobotcreate2::Battery battery;
		battery.header.stamp = current_time;
		battery.power_cord = roomba->power_cord_;
		battery.dock = roomba->dock_;
		battery.level = 100.0*(roomba->charge_/roomba->capacity_);
		if(last_charge > roomba->charge_) time_remaining = (int)(battery.level/((last_charge-roomba->charge_)/roomba->capacity_)/dt)/60;
		last_charge = roomba->charge_;
		battery.time_remaining = time_remaining;
		battery_pub.publish(battery);
	
		// ******************************************************************************************	
		//publish bumpers
        irobotcreate2::Bumper bumper;
		bumper.left.header.stamp = current_time;
		bumper.left.state = roomba->bumper_[LEFT];
		bumper.right.header.stamp = current_time;
		bumper.right.state = roomba->bumper_[RIGHT];
		bumper_pub.publish(bumper);
	
		// ******************************************************************************************	
		//publish buttons
        irobotcreate2::Buttons buttons;
		buttons.header.stamp = current_time;
		buttons.clean = roomba->buttons_[BUTTON_CLEAN];
		buttons.spot = roomba->buttons_[BUTTON_SPOT];
		buttons.dock = roomba->buttons_[BUTTON_DOCK];
		buttons.day = roomba->buttons_[BUTTON_DAY];
		buttons.hour = roomba->buttons_[BUTTON_HOUR];
		buttons.minute = roomba->buttons_[BUTTON_MINUTE];
		buttons.schedule = roomba->buttons_[BUTTON_SCHEDULE];
		buttons.clock = roomba->buttons_[BUTTON_CLOCK];
		buttons_pub.publish(buttons);

		// ******************************************************************************************
		//publish cliff
        irobotcreate2::RoombaIR cliff;
		cliff.header.stamp = current_time;

		cliff.header.frame_id = "base_cliff_left";
		cliff.state = roomba->cliff_[LEFT];
		cliff.signal = roomba->cliff_signal_[LEFT];
		cliff_pub.publish(cliff);

		cliff.header.frame_id = "base_cliff_front_left";
		cliff.state = roomba->cliff_[FRONT_LEFT];
		cliff.signal = roomba->cliff_signal_[FRONT_LEFT];
		cliff_pub.publish(cliff);

		cliff.header.frame_id = "base_cliff_front_right";
		cliff.state = roomba->cliff_[FRONT_RIGHT];
		cliff.signal = roomba->cliff_signal_[FRONT_RIGHT];
		cliff_pub.publish(cliff);

		cliff.header.frame_id = "base_cliff_right";
		cliff.state = roomba->cliff_[RIGHT];
		cliff.signal = roomba->cliff_signal_[RIGHT];
		cliff_pub.publish(cliff);

		// ******************************************************************************************
		//publish irbumper
        irobotcreate2::RoombaIR irbumper;
		irbumper.header.stamp = current_time;

		irbumper.header.frame_id = "base_irbumper_left";
		irbumper.state = roomba->ir_bumper_[LEFT];
		irbumper.signal = roomba->ir_bumper_signal_[LEFT];
		irbumper_pub.publish(irbumper);

		irbumper.header.frame_id = "base_irbumper_front_left";
		irbumper.state = roomba->ir_bumper_[FRONT_LEFT];
		irbumper.signal = roomba->ir_bumper_signal_[FRONT_LEFT];
		irbumper_pub.publish(irbumper);

		irbumper.header.frame_id = "base_irbumper_center_left";
		irbumper.state = roomba->ir_bumper_[CENTER_LEFT];
		irbumper.signal = roomba->ir_bumper_signal_[CENTER_LEFT];
		irbumper_pub.publish(irbumper);

		irbumper.header.frame_id = "base_irbumper_center_right";
		irbumper.state = roomba->ir_bumper_[CENTER_RIGHT];
		irbumper.signal = roomba->ir_bumper_signal_[CENTER_RIGHT];
		irbumper_pub.publish(irbumper);

		irbumper.header.frame_id = "base_irbumper_front_right";
		irbumper.state = roomba->ir_bumper_[FRONT_RIGHT];
		irbumper.signal = roomba->ir_bumper_signal_[FRONT_RIGHT];
		irbumper_pub.publish(irbumper);

		irbumper.header.frame_id = "base_irbumper_right";
		irbumper.state = roomba->ir_bumper_[RIGHT];
		irbumper.signal = roomba->ir_bumper_signal_[RIGHT];
		irbumper_pub.publish(irbumper);

		// ******************************************************************************************
		//publish irchar
        irobotcreate2::IRCharacter irchar;
		irchar.header.stamp = current_time;
		irchar.omni = roomba->ir_char_[OMNI];
		irchar.left = roomba->ir_char_[LEFT];
		irchar.right = roomba->ir_char_[RIGHT];
		irchar_pub.publish(irchar);

		// ******************************************************************************************
		//publish wheeldrop
        irobotcreate2::WheelDrop wheeldrop;
		wheeldrop.left.header.stamp = current_time;
		wheeldrop.left.state = roomba->wheel_drop_[LEFT];
		wheeldrop.right.header.stamp = current_time;
		wheeldrop.right.state = roomba->wheel_drop_[RIGHT];
		wheeldrop_pub.publish(wheeldrop);
		
		ros::spinOnce();
		r.sleep();
		
		if(first_loop) {roomba->startOI(true); first_loop=false;}
	}
	
	roomba->powerDown();
	roomba->closeSerialPort();
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboQuadrotorStateController::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  world = _model->GetWorld();

  // load parameters
  if (!_sdf->HasElement("robotNamespace"))
    namespace_.clear();
  else
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";

  if (!_sdf->HasElement("topicName"))
    velocity_topic_ = "cmd_vel";
  else
    velocity_topic_ = _sdf->GetElement("topicName")->Get<std::string>();

  if (!_sdf->HasElement("takeoffTopic"))
    takeoff_topic_ = "/ardrone/takeoff";
  else
    takeoff_topic_ = _sdf->GetElement("takeoffTopic")->Get<std::string>();

  if (!_sdf->HasElement("/ardrone/land"))
    land_topic_ = "/ardrone/land";
  else
    land_topic_ = _sdf->GetElement("landTopic")->Get<std::string>();

  if (!_sdf->HasElement("resetTopic"))
    reset_topic_ = "/ardrone/reset";
  else
    reset_topic_ = _sdf->GetElement("resetTopic")->Get<std::string>();

  if (!_sdf->HasElement("navdataTopic"))
    navdata_topic_ = "/ardrone/navdata";
  else
    navdata_topic_ = _sdf->GetElement("navdataTopic")->Get<std::string>();

  if (!_sdf->HasElement("imuTopic"))
    imu_topic_.clear();
  else
    imu_topic_ = _sdf->GetElement("imuTopic")->Get<std::string>();

  if (!_sdf->HasElement("sonarTopic"))
    sonar_topic_.clear();
  else
    sonar_topic_ = _sdf->GetElement("sonarTopic")->Get<std::string>();

  if (!_sdf->HasElement("contactTopic"))
	contact_topic_.clear();
  else
	contact_topic_ = _sdf->GetElement("contactTopic")->Get<std::string>();

  if (!_sdf->HasElement("stateTopic"))
    state_topic_.clear();
  else
    state_topic_ = _sdf->GetElement("stateTopic")->Get<std::string>();

  if (!_sdf->HasElement("bodyName"))
  {
    link = _model->GetLink();
    link_name_ = link->GetName();
  }
  else {
    link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
    link = boost::dynamic_pointer_cast<physics::Link>(world->GetEntity(link_name_));
  }

  if (!link)
  {
    ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
    return;
  }

  node_handle_ = new ros::NodeHandle(namespace_);

  // subscribe command: velocity control command
  if (!velocity_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<geometry_msgs::Twist>(
      velocity_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::VelocityCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    velocity_subscriber_ = node_handle_->subscribe(ops);
  }

  // subscribe command: take off command
  if (!takeoff_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
      takeoff_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::TakeoffCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    takeoff_subscriber_ = node_handle_->subscribe(ops);
  }

  // subscribe command: take off command
  if (!land_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
      land_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::LandCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    land_subscriber_ = node_handle_->subscribe(ops);
  }

  // subscribe command: take off command
  if (!reset_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
      reset_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::ResetCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    reset_subscriber_ = node_handle_->subscribe(ops);
  }

    m_navdataPub = node_handle_->advertise< ardrone_autonomy::Navdata >( navdata_topic_ , 25 );


  // subscribe imu
  if (!imu_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(
      imu_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::ImuCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    imu_subscriber_ = node_handle_->subscribe(ops);

    ROS_INFO_NAMED("quadrotor_state_controller", "Using imu information on topic %s as source of orientation and angular velocity.", imu_topic_.c_str());
  }

  // subscribe sonar senor info
  if (!sonar_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Range>(
      sonar_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::SonarCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    sonar_subscriber_ = node_handle_->subscribe(ops);

    ROS_INFO_NAMED("quadrotor_state_controller", "Using sonar information on topic %s as source of altitude.", sonar_topic_.c_str());
  }

  // subscribe contact senor info
  if (!contact_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Int32>(
      contact_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::ContactCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    contact_subscriber_ = node_handle_->subscribe(ops);

    ROS_INFO_NAMED("quadrotor_state_controller", "Using contact information on topic %s as source of collisions.", contact_topic_.c_str());
  }

  // subscribe state
  if (!state_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(
      state_topic_, 1,
      boost::bind(&GazeboQuadrotorStateController::StateCallback, this, _1),
      ros::VoidPtr(), &callback_queue_);
    state_subscriber_ = node_handle_->subscribe(ops);

    ROS_INFO_NAMED("quadrotor_state_controller", "Using state information on topic %s as source of state information.", state_topic_.c_str());
  }


  // for camera control
  // switch camera server
  std::string toggleCam_topic  = "ardrone/togglecam";
  ros::AdvertiseServiceOptions toggleCam_ops = ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
    toggleCam_topic,
    boost::bind(&GazeboQuadrotorStateController::toggleCamCallback, this, _1,_2),
    ros::VoidPtr(),
    &callback_queue_);

  toggleCam_service = node_handle_->advertiseService(toggleCam_ops);

  // camera image data
  std::string cam_out_topic  = "/ardrone/image_raw";
  std::string cam_front_in_topic = "/ardrone/front/image_raw";
  std::string cam_bottom_in_topic = "/ardrone/bottom/image_raw";
  std::string in_transport = "raw";

  camera_it_ = new image_transport::ImageTransport(*node_handle_);
  camera_publisher_ = camera_it_->advertise(cam_out_topic, 1);

  camera_front_subscriber_ = camera_it_->subscribe(
    cam_front_in_topic, 1,
    boost::bind(&GazeboQuadrotorStateController::CameraFrontCallback, this, _1),
    ros::VoidPtr(), in_transport);

  camera_bottom_subscriber_ = camera_it_->subscribe(
    cam_bottom_in_topic, 1,
    boost::bind(&GazeboQuadrotorStateController::CameraBottomCallback, this, _1),
    ros::VoidPtr(), in_transport);

  // camera image data
  std::string cam_info_out_topic  = "/ardrone/camera_info";
  std::string cam_info_front_in_topic = "/ardrone/front/camera_info";
  std::string cam_info_bottom_in_topic = "/ardrone/bottom/camera_info";

  camera_info_publisher_ = node_handle_->advertise<sensor_msgs::CameraInfo>(cam_info_out_topic,1);

  ros::SubscribeOptions cam_info_front_ops = ros::SubscribeOptions::create<sensor_msgs::CameraInfo>(
    cam_info_front_in_topic, 1,
    boost::bind(&GazeboQuadrotorStateController::CameraInfoFrontCallback, this, _1),
    ros::VoidPtr(), &callback_queue_);
  camera_info_front_subscriber_ = node_handle_->subscribe(cam_info_front_ops);

  ros::SubscribeOptions cam_info_bottom_ops = ros::SubscribeOptions::create<sensor_msgs::CameraInfo>(
    cam_info_bottom_in_topic, 1,
    boost::bind(&GazeboQuadrotorStateController::CameraInfoBottomCallback, this, _1),
    ros::VoidPtr(), &callback_queue_);
  camera_info_bottom_subscriber_ = node_handle_->subscribe(cam_info_bottom_ops);

  // callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorStateController::CallbackQueueThread,this ) );


  Reset();

  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  updateConnection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboQuadrotorStateController::Update, this));

  robot_current_state = LANDED_MODEL;
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, "roomba560_light_node");

    ROS_INFO("Roomba for ROS %.2f", NODE_VERSION);

    double last_x, last_y, last_yaw;
    double vel_x, vel_y, vel_yaw;
    double dt;

    ros::NodeHandle n;
    ros::NodeHandle private_nh("~");

    private_nh.param<std::string>("port", port, "/dev/ttyUSB0");
    private_nh.param<double>("test_vel", test_vel, 0.1);
    private_nh.param<double>("test_odom_th", test_odom_th, 0.02);

    test_vel_inc = test_vel / 20;

    roomba = new irobot::OpenInterface(port.c_str());

    ros::Publisher js_pub = n.advertise<sensor_msgs::JointState>("/joint_states", 50);

    sensor_msgs::JointState js;

    js.name.resize(2);
    js.position.resize(2);
    js.velocity.resize(2);
    js.effort.resize(2);

    js.name[0] = "left_wheel_joint";
    js.name[1] = "right_wheel_joint";

    js.position[0] = 0.0;
    js.position[1] = 0.0;

    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("/odom", 5);
    //tf::TransformBroadcaster odom_broadcaster;
    ros::Subscriber cmd_vel_sub  = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 1, cmdVelReceived);

    if( roomba->openSerialPort(true) == 0) ROS_INFO("Connected to Roomba.");
    else
    {
        ROS_FATAL("Could not connect to Roomba.");
        ROS_BREAK();
    }

    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();

    int heartvalue = 0;
    bool inc = true;

    bool packets_received = false;
    unsigned int packets_not_received = 0;

    unsigned int test_cnt = 0;

    double test_vel_tmp = 0.0;

    ros::Rate r(20.0);
    while(n.ok())
    {

        current_time = ros::Time::now();

        last_x = roomba->odometry_x_;
        last_y = roomba->odometry_y_;
        last_yaw = roomba->odometry_yaw_;

        if (!command_tested) {

            if (test_vel_tmp<test_vel) test_vel_tmp += test_vel_inc;

            roomba->drive(test_vel_tmp,0.0);
            roomba->setLeds(0, 0, 0, 0, 255, 255);

            if (roomba->odometry_x_ > test_odom_th) {

                roomba->drive(0.0,0.0);
                command_tested = true;
                ROS_INFO("Test of base driver was successful.");

            } else {

                if (test_cnt++ >= 20) { // 2s

                    roomba->powerDown();
                    roomba->closeSerialPort();
                    roomba_is_down = true;

                    ROS_ERROR("Test of base driver failed. Shutting down. Please press red button.");
                    ros::Duration dur(10);
                    dur.sleep();
                    break;

                }


            }

        } else {


            if(inc==true) heartvalue += 20;
            else heartvalue -= 20;

            if(heartvalue>=255) {

                heartvalue = 255;
                inc=false;
            }

            if(heartvalue<=0) {

                heartvalue = 0;
                inc=true;

            }

            roomba->setLeds(0, 0, 0, 0, (unsigned char)heartvalue, 255);

        }



        if( (roomba->getSensorPackets(100) == -1) && (packets_not_received++ >= 5) ) {

            roomba->powerDown();
            roomba->closeSerialPort();
            roomba_is_down = true;

            ROS_ERROR("Could not retrieve sensor packets. Shutting down. Please reset robot.");
            ros::Duration dur(20);

            dur.sleep();

            break;

        } else {

            if (!packets_received) {

                ROS_INFO("We are receiving sensor packets!");

                packets_received = true;

            }

            packets_not_received = 0;

            roomba->calculateOdometry();

        }

        dt = (current_time - last_time).toSec();

        if (command_tested) roomba->drive(g_lin, g_ang);

        vel_x = (roomba->odometry_x_ - last_x)/dt;
        vel_y = (roomba->odometry_y_ - last_y)/dt;
        vel_yaw = (roomba->odometry_yaw_ - last_yaw)/dt;

        //since all odometry is 6DOF we'll need a quaternion created from yaw
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_);

        //first, we'll publish the transform over tf
        /*geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = current_time;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_link";

        odom_trans.transform.translation.x = roomba->odometry_x_;
        odom_trans.transform.translation.y = roomba->odometry_y_;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;*/

        //send the transform
        //odom_broadcaster.sendTransform(odom_trans);

        //next, we'll publish the odometry message over ROS
        nav_msgs::Odometry odom;
        odom.header.stamp = current_time;
        //odom.header.frame_id = "odom";
        odom.header.frame_id = "odom_combined";

        //set the position
        odom.pose.pose.position.x = roomba->odometry_x_;
        odom.pose.pose.position.y = roomba->odometry_y_;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;

        //set the velocity
        //odom.child_frame_id = "base_link";
        odom.child_frame_id = "base_footprint";
        odom.twist.twist.linear.x = vel_x;
        odom.twist.twist.linear.y = vel_y;
        odom.twist.twist.angular.z = vel_yaw;

        // ZdenekM -> added covariance, needed for robot_pose_ekf...
        odom.pose.covariance = boost::assign::list_of	(1e-3) (0) (0)  (0)  (0)  (0)
                               (0) (1e-3)  (0)  (0)  (0)  (0)
                               (0)   (0)  (1e6) (0)  (0)  (0)
                               (0)   (0)   (0) (1e6) (0)  (0)
                               (0)   (0)   (0)  (0) (1e6) (0)
                               (0)   (0)   (0)  (0)  (0)  (1e3) ;

        odom.twist.covariance = boost::assign::list_of	(1e-3) (0) (0)  (0)  (0)  (0)
                                (0) (1e-3)  (0)  (0)  (0)  (0)
                                (0)   (0)  (1e6) (0)  (0)  (0)
                                (0)   (0)   (0) (1e6) (0)  (0)
                                (0)   (0)   (0)  (0) (1e6) (0)
                                (0)   (0)   (0)  (0)  (0)  (1e3) ;

        //publish the message
        if (command_tested) {

            odom_pub.publish(odom);

            // stop if there is no command for more than 0.5s
            if ((ros::Time::now() - latest_command) > ros::Duration(0.5)) {

                g_ang = 0.0;
                g_lin = 0.0;
                roomba->drive(0.0,0.0);

            }

        }

        js_pub.publish(js);



        ros::spinOnce();
        r.sleep();
    }

    roomba->setLeds(0, 0, 0, 0, 0, 0);

    if (!roomba_is_down) {

        roomba->powerDown();
        roomba->closeSerialPort();

    }
}
int main(int argc, char ** argv)
{
  //Initialize ROS
  ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName);

  if(argc == 11)
  {
    ros::Duration sleeper(atof(argv[10])/1000.0);

    if (strcmp(argv[8], argv[9]) == 0)
      ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[8], argv[9]);

    TransformSender tf_sender(atof(argv[1]), atof(argv[2]), atof(argv[3]),
                              atof(argv[4]), atof(argv[5]), atof(argv[6]), atof(argv[7]),
                              ros::Time() + sleeper, //Future dating to allow slower sending w/o timeout
                              argv[8], argv[9]);



    while(tf_sender.node_.ok())
    {
      tf_sender.send(ros::Time::now() + sleeper);
      ROS_DEBUG("Sending transform from %s with parent %s\n", argv[8], argv[9]);
      sleeper.sleep();
    }

    return 0;
  } 
  else if (argc == 10)
  {
    ros::Duration sleeper(atof(argv[9])/1000.0);

    if (strcmp(argv[7], argv[8]) == 0)
      ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[7], argv[8]);

    TransformSender tf_sender(atof(argv[1]), atof(argv[2]), atof(argv[3]),
                              atof(argv[4]), atof(argv[5]), atof(argv[6]),
                              ros::Time() + sleeper, //Future dating to allow slower sending w/o timeout
                              argv[7], argv[8]);



    while(tf_sender.node_.ok())
    {
      tf_sender.send(ros::Time::now() + sleeper);
      ROS_DEBUG("Sending transform from %s with parent %s\n", argv[7], argv[8]);
      sleeper.sleep();
    }

    return 0;

  }
  else
  {
    printf("A command line utility for manually sending a transform.\n");
    printf("It will periodicaly republish the given transform. \n");
    printf("Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id  period(milliseconds) \n");
    printf("OR \n");
    printf("Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id  period(milliseconds) \n");
    printf("\nThis transform is the transform of the coordinate frame from frame_id into the coordinate frame \n");
    printf("of the child_frame_id.  \n");
    ROS_ERROR("static_transform_publisher exited due to not having the right number of arguments");
    return -1;
  }


};
示例#8
0
  void ARSinglePublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
  {
    ARUint8 *dataPtr;
    ARMarkerInfo *marker_info;
    int marker_num;
    int i, k;

    /* Get the image from ROSTOPIC
     * NOTE: the dataPtr format is BGR because the ARToolKit library was
     * build with V4L, dataPtr format change according to the 
     * ARToolKit configure option (see config.h).*/
    try
    {
      capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
    }
    catch (sensor_msgs::CvBridgeException & e)
    {
      ROS_ERROR ("Could not convert from '%s' to 'bgr8'.", image_msg->encoding.c_str ());
    }
    //cvConvertImage(capture_,capture_,CV_CVTIMG_FLIP); //flip image
    dataPtr = (ARUint8 *) capture_->imageData;

    // detect the markers in the video frame 
    if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
    {
      ROS_FATAL ("arDetectMarker failed");
      ROS_BREAK ();             // FIXME: I don't think this should be fatal... -Bill
    }

    // check for known patterns
    k = -1;
    for (i = 0; i < marker_num; i++)
    {
      if (marker_info[i].id == patt_id_)
      {
        ROS_DEBUG ("Found pattern: %d ", patt_id_);

        // make sure you have the best pattern (highest confidence factor)
        if (k == -1)
          k = i;
        else if (marker_info[k].cf < marker_info[i].cf)
          k = i;
      }
    }

    if (k != -1)
    {
      // **** get the transformation between the marker and the real camera
      double arQuat[4], arPos[3];

      if (!useHistory_ || contF == 0)
        arGetTransMat (&marker_info[k], marker_center_, markerWidth_, marker_trans_);
      else
        arGetTransMatCont (&marker_info[k], marker_trans_, marker_center_, markerWidth_, marker_trans_);

      contF = 1;

      //arUtilMatInv (marker_trans_, cam_trans);
      arUtilMat2QuatPos (marker_trans_, arQuat, arPos);

      // **** convert to ROS frame

      double quat[4], pos[3];
    
      pos[0] = arPos[0] * AR_TO_ROS;
      pos[1] = arPos[1] * AR_TO_ROS;
      pos[2] = arPos[2] * AR_TO_ROS;

      quat[0] = -arQuat[0];
      quat[1] = -arQuat[1];
      quat[2] = -arQuat[2];
      quat[3] = arQuat[3];

      ROS_DEBUG (" QUAT: Pos x: %3.5f  y: %3.5f  z: %3.5f", pos[0], pos[1], pos[2]);
      ROS_DEBUG ("     Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);

      // **** publish the marker

		  ar_pose_marker_.header.frame_id = image_msg->header.frame_id;
		  ar_pose_marker_.header.stamp    = image_msg->header.stamp;
		  ar_pose_marker_.id              = marker_info->id;

		  ar_pose_marker_.pose.pose.position.x = pos[0];
		  ar_pose_marker_.pose.pose.position.y = pos[1];
		  ar_pose_marker_.pose.pose.position.z = pos[2];

		  ar_pose_marker_.pose.pose.orientation.x = quat[0];
		  ar_pose_marker_.pose.pose.orientation.y = quat[1];
		  ar_pose_marker_.pose.pose.orientation.z = quat[2];
		  ar_pose_marker_.pose.pose.orientation.w = quat[3];
		
		  ar_pose_marker_.confidence = marker_info->cf;

		  arMarkerPub_.publish(ar_pose_marker_);
		  ROS_DEBUG ("Published ar_single marker");
		
      // **** publish transform between camera and marker

      btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
      btVector3 origin(pos[0], pos[1], pos[2]);
      btTransform t(rotation, origin);

      if(publishTf_)
      {
        if(reverse_transform_)
        {
          tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame_.c_str(), image_msg->header.frame_id);
          broadcaster_.sendTransform(markerToCam);
        } else {
          tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame_.c_str());
          broadcaster_.sendTransform(camToMarker);
        }
      }

      // **** publish visual marker

      if(publishVisualMarkers_)
      {
        btVector3 markerOrigin(0, 0, 0.25 * markerWidth_ * AR_TO_ROS);
        btTransform m(btQuaternion::getIdentity(), markerOrigin);
        btTransform markerPose = t * m; // marker pose in the camera frame
      
        tf::poseTFToMsg(markerPose, rvizMarker_.pose);

			  rvizMarker_.header.frame_id = image_msg->header.frame_id;
			  rvizMarker_.header.stamp = image_msg->header.stamp;
			  rvizMarker_.id = 1;

			  rvizMarker_.scale.x = 1.0 * markerWidth_ * AR_TO_ROS;
			  rvizMarker_.scale.y = 1.0 * markerWidth_ * AR_TO_ROS;
			  rvizMarker_.scale.z = 0.5 * markerWidth_ * AR_TO_ROS;
			  rvizMarker_.ns = "basic_shapes";
			  rvizMarker_.type = visualization_msgs::Marker::CUBE;
			  rvizMarker_.action = visualization_msgs::Marker::ADD;
			  rvizMarker_.color.r = 0.0f;
			  rvizMarker_.color.g = 1.0f;
			  rvizMarker_.color.b = 0.0f;
			  rvizMarker_.color.a = 1.0;
			  rvizMarker_.lifetime = ros::Duration();
			
			  rvizMarkerPub_.publish(rvizMarker_);
			  ROS_DEBUG ("Published visual marker");
      }
    }
    else
    {
      contF = 0;
      ROS_DEBUG ("Failed to locate marker");
    }
  }
bool TreeKinematics::readJoints(urdf::Model &robot_model,
                                KDL::Tree &kdl_tree,
                                std::string &tree_root_name,
                                unsigned int &nr_of_jnts,
                                KDL::JntArray &joint_min,
                                KDL::JntArray &joint_max,
                                KDL::JntArray &joint_vel_max)
{
  KDL::SegmentMap segmentMap;
  segmentMap = kdl_tree.getSegments();
  tree_root_name = kdl_tree.getRootSegment()->second.segment.getName();
  nr_of_jnts = kdl_tree.getNrOfJoints();
  ROS_DEBUG( "the tree's number of joints: [%d]", nr_of_jnts );
  joint_min.resize(nr_of_jnts);
  joint_max.resize(nr_of_jnts);
  joint_vel_max.resize(nr_of_jnts);
  info_.joint_names.resize(nr_of_jnts);
  info_.limits.resize(nr_of_jnts);

  // The following walks through all tree segments, extracts their joints except joints of KDL::None and extracts
  // the information about min/max position and max velocity of the joints not of type urdf::Joint::UNKNOWN or
  // urdf::Joint::FIXED
  ROS_DEBUG("Extracting all joints from the tree, which are not of type KDL::Joint::None.");
  boost::shared_ptr<const urdf::Joint> joint;
  for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end(); ++seg_it)
  {
    if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None)
    {
      // check, if joint can be found in the URDF model of the robot
      joint = robot_model.getJoint(seg_it->second.segment.getJoint().getName().c_str());
      if (!joint)
      {
        ROS_FATAL("Joint '%s' has not been found in the URDF robot model!", joint->name.c_str());
        return false;
      }
      // extract joint information
      if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
      {
        ROS_DEBUG( "getting information about joint: [%s]", joint->name.c_str() );
        double lower = 0.0, upper = 0.0, vel_limit = 0.0;
        unsigned int has_pos_limits = 0, has_vel_limits = 0;

        if ( joint->type != urdf::Joint::CONTINUOUS )
        {
          ROS_DEBUG("joint is not continuous.");
          lower = joint->limits->lower;
          upper = joint->limits->upper;
          has_pos_limits = 1;
          if (joint->limits->velocity)
          {
            has_vel_limits = 1;
            vel_limit = joint->limits->velocity;
            ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
          }
          else
          {
            has_vel_limits = 0;
            vel_limit = 0.0;
            ROS_DEBUG("joint has no velocity limit.");
          }
        }
        else
        {
          ROS_DEBUG("joint is continuous.");
          lower = -M_PI;
          upper = M_PI;
          has_pos_limits = 0;
          if(joint->limits && joint->limits->velocity)
          {
            has_vel_limits = 1;
            vel_limit = joint->limits->velocity;
            ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
          }
          else
          {
            has_vel_limits = 0;
            vel_limit = 0.0;
            ROS_DEBUG("joint has no velocity limit.");
          }
        }

        joint_min(seg_it->second.q_nr) = lower;
        joint_max(seg_it->second.q_nr) = upper;
        joint_vel_max(seg_it->second.q_nr) = vel_limit;
        ROS_DEBUG("pos_min = %f, pos_max = %f, vel_max = %f", lower, upper, vel_limit);

        info_.joint_names[seg_it->second.q_nr] = joint->name;
        info_.limits[seg_it->second.q_nr].joint_name = joint->name;
        info_.limits[seg_it->second.q_nr].has_position_limits = has_pos_limits;
        info_.limits[seg_it->second.q_nr].min_position = lower;
        info_.limits[seg_it->second.q_nr].max_position = upper;
        info_.limits[seg_it->second.q_nr].has_velocity_limits = has_vel_limits;
        info_.limits[seg_it->second.q_nr].max_velocity = vel_limit;
      }
    }
  }
  return true;
}
OPTCalibrationNode::OPTCalibrationNode(const ros::NodeHandle & node_handle)
  : node_handle_(node_handle),
    image_transport_(node_handle),
    world_computation_(FIRST_SENSOR),
    fixed_sensor_pose_(Pose::Identity())
{
  action_sub_ = node_handle_.subscribe("action", 1, &OPTCalibrationNode::actionCallback, this);
  status_pub_ = node_handle_.advertise<opt_msgs::CalibrationStatus>("status", 1);

  std::string world_computation_s;
  node_handle_.param("world_computation", world_computation_s, std::string("first_sensor"));
  if (world_computation_s == "first_sensor")
    world_computation_ = FIRST_SENSOR;
  else if (world_computation_s == "last_checkerboard")
    world_computation_ = LAST_CHECKERBOARD;
  else if (world_computation_s == "update")
    world_computation_ = UPDATE;
  else
    ROS_FATAL_STREAM("\"world_computation\" parameter value not valid. Please use \"first_sensor\", \"last_checkerboard\" or \"manual\".");

  std::string fixed_sensor_frame_id;
  if (world_computation_ == UPDATE)
  {
    if (not node_handle_.getParam("fixed_sensor/name", fixed_sensor_frame_id))
      ROS_FATAL_STREAM("No \"fixed_sensor/name\" parameter found!!");

    std::string pose_s = "/poses/" + fixed_sensor_frame_id;

    if (not node_handle_.hasParam(pose_s))
      ROS_FATAL_STREAM("No \"" << pose_s << "\" parameter found!! Has \"conf/camera_poses.yaml\" been loaded with \"rosparam load ...\"?");

    Translation3 t;
    node_handle_.getParam(pose_s + "/translation/x", t.x());
    node_handle_.getParam(pose_s + "/translation/y", t.y());
    node_handle_.getParam(pose_s + "/translation/z", t.z());

    Quaternion q;
    node_handle_.getParam(pose_s + "/rotation/x", q.x());
    node_handle_.getParam(pose_s + "/rotation/y", q.y());
    node_handle_.getParam(pose_s + "/rotation/z", q.z());
    node_handle_.getParam(pose_s + "/rotation/w", q.w());

    fixed_sensor_pose_.linear() = q.toRotationMatrix();
    fixed_sensor_pose_.translation() = t.vector();
  }

  node_handle_.param("num_sensors", num_sensors_, 0);

  double cell_width, cell_height;
  int rows, cols;

  bool cb_ok = true;
  cb_ok = cb_ok and node_handle_.getParam("cell_width", cell_width);
  cb_ok = cb_ok and node_handle_.getParam("cell_height", cell_height);
  cb_ok = cb_ok and node_handle_.getParam("rows", rows);
  cb_ok = cb_ok and node_handle_.getParam("cols", cols);
  if (not cb_ok)
    ROS_FATAL("Checkerboard parameter missing! Please set \"rows\", \"cols\", \"cell_width\" and \"cell_height\".");

  checkerboard_ = boost::make_shared<Checkerboard>(cols, rows, cell_width, cell_height);
  checkerboard_->setFrameId("/checkerboard");

  for (int i = 0; i < num_sensors_; ++i)
  {
    std::stringstream ss;

    std::string type_s;
    ss.str("");
    ss << "sensor_" << i << "/type";
    if (not node_handle_.getParam(ss.str(), type_s))
      ROS_FATAL_STREAM("No \"" << ss.str() << "\" parameter found!!");

//    SensorNode::SensorType type;
//    if (type_s == "pinhole_rgb")
//      type = SensorNode::PINHOLE_RGB;
//    else if (type_s == "kinect_depth")
//      type = SensorNode::KINECT_DEPTH;
//    else if (type_s == "tof_depth")
//      type = SensorNode::TOF_DEPTH;
//    else
//      ROS_FATAL_STREAM("\"" << ss.str() << "\" parameter value not valid. Please use \"pinhole_rgb\", \"kinect_depth\" or \"tof_depth\".");

    ss.str("");
    ss << "/sensor_" << i;
    std::string frame_id = ss.str();

    ss.str("");
    ss << "sensor_" << i << "/name";
    node_handle_.param(ss.str(), frame_id, frame_id);

    ROSDevice::Ptr ros_device;

    if (type_s == "pinhole_rgb")
    {
      PinholeRGBDevice::Ptr device = boost::make_shared<PinholeRGBDevice>(frame_id);
      pinhole_vec_.push_back(device);
      ros_device = device;
      ROS_INFO_STREAM(device->frameId() << " added.");
      if (world_computation_ == UPDATE)
      {
        ROS_INFO_STREAM(frame_id << " == " << fixed_sensor_frame_id << "?");
        if (frame_id == fixed_sensor_frame_id)
           fixed_sensor_ = device->sensor();
      }
    }
    else if (type_s == "kinect1")
    {
      KinectDevice::Ptr device = boost::make_shared<KinectDevice>(frame_id, frame_id + "_depth");
      kinect_vec_.push_back(device);
      ros_device = device;
      if (world_computation_ == UPDATE)
      {
        ROS_INFO_STREAM(frame_id << " == " << fixed_sensor_frame_id << "?");
        if (frame_id == fixed_sensor_frame_id)
           fixed_sensor_ = device->colorSensor();
      }
    }
    else if (type_s == "sr4500")
    {
      SwissRangerDevice::Ptr device = boost::make_shared<SwissRangerDevice>(frame_id);
      swiss_ranger_vec_.push_back(device);
      ros_device = device;
      if (world_computation_ == UPDATE)
      {
        ROS_INFO_STREAM(frame_id << " == " << fixed_sensor_frame_id << "?");
        if (frame_id == fixed_sensor_frame_id)
           fixed_sensor_ = device->depthSensor();
      }
    }
    else
    {
      ROS_FATAL_STREAM("\"" << ss.str() << "\" parameter value not valid. Please use \"pinhole_rgb\", \"kinect\" or \"swiss_ranger\".");
    }

    ss.str("");
    ss << "sensor_" << i;
    ros_device->createSubscribers(node_handle_, image_transport_, ss.str());

  }

  if (world_computation_ == UPDATE and not fixed_sensor_)
    ROS_FATAL_STREAM("Wrong \"fixed_sensor/name\" parameter provided: " << fixed_sensor_frame_id);
}
int main(int argc, char **argv)
{
  // Init the ROS node
  ros::init (argc, argv, "move_right_arm_joint_goal_test");

  // Precondition: Valid clock
  ros::NodeHandle nh;
  if (!ros::Time::waitForValid(ros::WallDuration(5.0))) // NOTE: Important when using simulated clock
  {
    ROS_FATAL("Timed-out waiting for valid time.");
    return EXIT_FAILURE;
  }

  // Action client for sending motion planing requests
  actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm_torso", true);

  // Wait for the action client to be connected to the server
  ROS_INFO("Connecting to server...");
  if (!move_arm.waitForServer(ros::Duration(5.0)))
  {
    ROS_ERROR("Timed-out waiting for the move_arm action server.");
    return EXIT_FAILURE;
  }
  ROS_INFO("Connected to server.");

  // Prepare motion plan request with joint-space goal
  arm_navigation_msgs::MoveArmGoal goal;
  std::vector<std::string> names(9);
  names[0] = "torso_1_joint";
  names[1] = "torso_2_joint";
  names[2] = "arm_right_1_joint";
  names[3] = "arm_right_2_joint";
  names[4] = "arm_right_3_joint";
  names[5] = "arm_right_4_joint";
  names[6] = "arm_right_5_joint";
  names[7] = "arm_right_6_joint";
  names[8] = "arm_right_7_joint";

  goal.motion_plan_request.group_name = "right_arm_torso";
  goal.motion_plan_request.num_planning_attempts = 1;
  goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

  goal.motion_plan_request.planner_id= std::string("");
  goal.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  goal.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());

  for (unsigned int i = 0 ; i < goal.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
  {
    goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
    goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
    goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.05;
    goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.05;
  }

  goal.motion_plan_request.goal_constraints.joint_constraints[0].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[1].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[2].position =  1.2;
  goal.motion_plan_request.goal_constraints.joint_constraints[3].position =  0.15;
  goal.motion_plan_request.goal_constraints.joint_constraints[4].position = -1.5708;
  goal.motion_plan_request.goal_constraints.joint_constraints[5].position =  1.3;
  goal.motion_plan_request.goal_constraints.joint_constraints[6].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[7].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[8].position =  0.0;

  // Send motion plan request
  if (nh.ok())
  {
    bool finished_within_time = false;
    move_arm.sendGoal(goal);
    finished_within_time = move_arm.waitForResult(ros::Duration(15.0));
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving joint-space goal.");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }
  ros::shutdown();
}
int main(int argc, char** argv)
{
  ros::init(argc, argv, std::string("owd"));

#ifdef OWD_RT
  if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1){
    ROS_FATAL("owd: mlockall failed: ");
    return NULL;
  }
#else // OWD_RT
#ifndef OWDSIM
  // increase the process priority so that it can still do effective control
  // on a non-realtime system
  if (setpriority(PRIO_PROCESS,0,-5)) {
    ROS_WARN("Could not elevate process scheduling priority; will be more vulnerable to heartbeat faults on a heavily loaded system");
    ROS_WARN("Please make sure the executable is set to suid root");
  }
#endif // OWDSIM
#endif // OWD_RT


  // read parameters and set wam options

  ros::NodeHandle n("~");
  std::string calibration_filename;
  int canbus_number;
  std::string hand_type;
  bool forcetorque;
  bool modified_j1;
  int pub_freq;  // DEPRECATED
  int wam_pub_freq;
  int hand_pub_freq;
  int ft_pub_freq;
  int tactile_pub_freq;
  n.param("calibration_file",calibration_filename,std::string("wam_joint_calibrations"));
  n.param("canbus_number",canbus_number,0);
  n.param("hand_type",hand_type,std::string("none"));
  n.param("forcetorque_sensor",forcetorque,false);
  n.param("modified_j1",modified_j1,false);
  int wam_freq_default=10;
  int hand_freq_default=10;
 if (n.getParam("publish_frequency",pub_freq)) {
    ROS_WARN("Parameter publish_frequency has been deprecated.  Please use wam_publish_frequency, hand_publish_frequency, ft_publish_frequency, and tactile_publish_frequency.");
    // we'll use the deprecated value to set the defaults for the
    // individual hand and wam frequencies, so that it will only be
    // used if the newer params are not specified
    wam_freq_default=hand_freq_default=pub_freq;
  }
  n.param("wam_publish_frequency",wam_pub_freq,wam_freq_default);
  n.param("hand_publish_frequency",hand_pub_freq,hand_freq_default);
  n.param("ft_publish_frequency",ft_pub_freq,10);
  n.param("tactile_publish_frequency",tactile_pub_freq,10);

  if (wam_pub_freq > 500) {
    ROS_WARN("value of wam_publish_frequency exceeds maximum sensor rate; capping to 500Hz");
    wam_pub_freq = 500;
  }
  if (hand_pub_freq > 40) {
    ROS_WARN("value of hand_publish_frequency exceeds maximum sensor rate; capping to 40Hz");
    hand_pub_freq = 40;
  }
  if (ft_pub_freq > 500) {
    ROS_WARN("value of ft_publish_frequency exceeds maximum sensor rate; capping to 500Hz");
    ft_pub_freq = 500;
  }
  if (tactile_pub_freq > 40) {
    ROS_WARN("value of tactile_publish_frequency exceeds maximum sensor rate; capping to 40Hz");
    tactile_pub_freq = 40;
  }
  
  ROS_DEBUG("Using CANbus number %d",canbus_number);

  int BH_model(0);
  bool tactile(false);
  if (! hand_type.compare(0,3,"280")) {
    BH_model=280;
    if (! hand_type.compare("280+TACT")) {
      ROS_DEBUG("Expecting tactile sensors on this hand");
      tactile=true;
    }
  } else if (! hand_type.compare(0,3,"260")) {
    BH_model=260;
  } else if (! hand_type.compare(0,7,"Robotiq")) {
    BH_model=998;
  } else if (! hand_type.compare(0,29,"darpa_arms_calibration_target")) {
    BH_model=999;
  } else if (hand_type.compare(0,4, "none")) { // note the absence of the !
    ROS_FATAL("Unknown hand type \"%s\"; cannot continue with unknown mass properties",
	      hand_type.c_str());
    exit(1);
  }


}
// load robot footprint from costmap_2d_ros to keep same footprint format
std::vector<geometry_msgs::Point> CollisionVelocityFilter::loadRobotFootprint(ros::NodeHandle node){
  std::vector<geometry_msgs::Point> footprint;
  geometry_msgs::Point pt;
  double padding;

  std::string padding_param, footprint_param;
  if(!node.searchParam("footprint_padding", padding_param))
    padding = 0.01;
  else
    node.param(padding_param, padding, 0.01);

  //grab the footprint from the parameter server if possible
  XmlRpc::XmlRpcValue footprint_list;
  std::string footprint_string;
  std::vector<std::string> footstring_list;
  if(node.searchParam("footprint", footprint_param)){
    node.getParam(footprint_param, footprint_list);
    if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) {
      footprint_string = std::string(footprint_list);

      //if there's just an empty footprint up there, return
      if(footprint_string == "[]" || footprint_string == "")
        return footprint;

      boost::erase_all(footprint_string, " ");

      boost::char_separator<char> sep("[]");
      boost::tokenizer<boost::char_separator<char> > tokens(footprint_string, sep);
      footstring_list = std::vector<std::string>(tokens.begin(), tokens.end());
    }
    //make sure we have a list of lists
    if(!(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_list.size() > 2) && !(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString && footstring_list.size() > 5)){
      ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", footprint_param.c_str(), std::string(footprint_list).c_str());
      throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
    }

    if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray) {
      for(int i = 0; i < footprint_list.size(); ++i){
        //make sure we have a list of lists of size 2
        XmlRpc::XmlRpcValue point = footprint_list[i];
        if(!(point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2)){
          ROS_FATAL("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
          throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
        }

        //make sure that the value we're looking at is either a double or an int
        if(!(point[0].getType() == XmlRpc::XmlRpcValue::TypeInt || point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
          ROS_FATAL("Values in the footprint specification must be numbers");
          throw std::runtime_error("Values in the footprint specification must be numbers");
        }
        pt.x = point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[0]) : (double)(point[0]);
        pt.x += sign(pt.x) * padding;

        //make sure that the value we're looking at is either a double or an int
        if(!(point[1].getType() == XmlRpc::XmlRpcValue::TypeInt || point[1].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
          ROS_FATAL("Values in the footprint specification must be numbers");
          throw std::runtime_error("Values in the footprint specification must be numbers");
        }
        pt.y = point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[1]) : (double)(point[1]);
        pt.y += sign(pt.y) * padding;

        footprint.push_back(pt);

        node.deleteParam(footprint_param);
        std::ostringstream oss;
        bool first = true;
        BOOST_FOREACH(geometry_msgs::Point p, footprint) {
          if(first) {
            oss << "[[" << p.x << "," << p.y << "]";
            first = false;
          }
          else {
            oss << ",[" << p.x << "," << p.y << "]";
          }
        }
        oss << "]";
        node.setParam(footprint_param, oss.str().c_str());
        node.setParam("footprint", oss.str().c_str());
      }
    }

    else if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) {
int main(int argc, char** argv)
{
    ros::init(argc, argv, "raw_marker_bridge");
    ROS_INFO("Starting raw marker bridge...");
    ros::NodeHandle nh;
    ros::NodeHandle nhp("~");
    ROS_INFO("Loading parameters...");
    std::string tracker_hostname;
    std::string tracker_port;
    std::string tracker_frame_name;
    std::string tracker_name;
    std::string tracker_topic;
    std::string stream_mode;
    nhp.param(std::string("tracker_hostname"), tracker_hostname, std::string("192.168.2.161"));
    nhp.param(std::string("tracker_port"), tracker_port, std::string("801"));
    nhp.param(std::string("tracker_frame_name"), tracker_frame_name, std::string("mocap_world"));
    nhp.param(std::string("tracker_name"), tracker_name, std::string("vicon"));
    nhp.param(std::string("tracker_topic"), tracker_topic, std::string("mocap_markers"));
    nhp.param(std::string("stream_mode"), stream_mode, std::string("ServerPush"));
    // Check the stream mode
    if (stream_mode == std::string("ServerPush") || stream_mode == std::string("ClientPullPreFetch") || stream_mode == std::string("ClientPull"))
    {
        ROS_INFO("Starting in %s mode", stream_mode.c_str());
    }
    else
    {
        ROS_FATAL("Invalid stream mode %s, valid options are ServerPush, ClientPullPreFetch, and ClientPull", stream_mode.c_str());
        exit(-1);
    }
    // Assemble the full hostname
    tracker_hostname = tracker_hostname + ":" + tracker_port;
    ROS_INFO("Connecting to Vicon Tracker (DataStream SDK) at hostname %s", tracker_hostname.c_str());
    // Make the ROS publisher
    ros::Publisher mocap_pub = nh.advertise<lightweight_vicon_bridge::MocapMarkerArray>(tracker_topic, 1, false);
    // Initialize the DataStream SDK
    ViconDataStreamSDK::CPP::Client sdk_client;
    ROS_INFO("Connecting to server...");
    sdk_client.Connect(tracker_hostname);
    usleep(10000);
    while (!sdk_client.IsConnected().Connected && ros::ok())
    {
        ROS_WARN("...taking a while to connect, trying again...");
        sdk_client.Connect(tracker_hostname);
        usleep(10000);
    }
    ROS_INFO("...connected!");
    // Enable data
    sdk_client.EnableUnlabeledMarkerData();
    // Set the axes (right-handed, X-forwards, Y-left, Z-up, same as ROS)
    sdk_client.SetAxisMapping(ViconDataStreamSDK::CPP::Direction::Forward, ViconDataStreamSDK::CPP::Direction::Left, ViconDataStreamSDK::CPP::Direction::Up);
    // Set streaming mode
    if (stream_mode == "ServerPush")
    {
        sdk_client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ServerPush);
    }
    else if (stream_mode == "ClientPullPreFetch")
    {
        sdk_client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch);
    }
    else if (stream_mode == "ClientPull")
    {
        sdk_client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ClientPull);
    }
    else
    {
        ROS_FATAL("Invalid stream mode");
        exit(-2);
    }
    // Start streaming data
    ROS_INFO("Streaming data...");
    while (ros::ok())
    {
        // Get a new frame and process it
        if (sdk_client.GetFrame().Result == ViconDataStreamSDK::CPP::Result::Success)
        {
            double total_latency = sdk_client.GetLatencyTotal().Total;
            ros::Duration latency_duration(total_latency);
            ros::Time current_time = ros::Time::now();
            ros::Time frame_time = current_time - latency_duration;
            lightweight_vicon_bridge::MocapMarkerArray state_msg;
            state_msg.header.frame_id = tracker_frame_name;
            state_msg.header.stamp = frame_time;
            state_msg.tracker_name = tracker_name;
            // Get the unlabeled markers
            unsigned int unlabelled_markers = sdk_client.GetUnlabeledMarkerCount().MarkerCount;
            for (unsigned int idx = 0; idx < unlabelled_markers; idx++)
            {
                ViconDataStreamSDK::CPP::Output_GetUnlabeledMarkerGlobalTranslation position = sdk_client.GetUnlabeledMarkerGlobalTranslation(idx);
                lightweight_vicon_bridge::MocapMarker marker_msg;
                marker_msg.index = idx;
                marker_msg.position.x = position.Translation[0] * 0.001;
                marker_msg.position.y = position.Translation[1] * 0.001;
                marker_msg.position.z = position.Translation[2] * 0.001;
                state_msg.markers.push_back(marker_msg);
            }
            mocap_pub.publish(state_msg);
        }
        // Handle ROS stuff
        ros::spinOnce();
    }
    sdk_client.DisableUnlabeledMarkerData();
    sdk_client.Disconnect();
    return 0;
}
void CommonDataSubscriber::setupRGBDCallbacks(
		ros::NodeHandle & nh,
		ros::NodeHandle & pnh,
		bool subscribeOdom,
		bool subscribeUserData,
		bool subscribeScan2d,
		bool subscribeScan3d,
		bool subscribeOdomInfo,
		int queueSize,
		bool approxSync)
{
	ROS_INFO("Setup rgbd callback");

	if(subscribeOdom || subscribeUserData || subscribeScan2d || subscribeScan3d || subscribeOdomInfo)
	{
		rgbdSubs_.resize(1);
		rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
		rgbdSubs_[0]->subscribe(nh, "rgbd_image", 1);


		if(subscribeOdom && subscribeUserData)
		{
			odomSub_.subscribe(nh, "odom", 1);
			userDataSub_.subscribe(nh, "user_data", 1);
			if(subscribeScan2d)
			{
				subscribedToScan2d_ = true;
				scanSub_.subscribe(nh, "scan", 1);
				SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
			}
			else if(subscribeScan3d)
			{
				subscribedToScan3d_ = true;
				scan3dSub_.subscribe(nh, "scan_cloud", 1);
				SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
			}
			else if(subscribeOdomInfo)
			{
				subscribedToOdomInfo_ = true;
				odomInfoSub_.subscribe(nh, "odom_info", 1);
				SYNC_DECL4(rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
			}
			else
			{
				SYNC_DECL3(rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
			}
		}
		else if(subscribeOdom)
		{
			odomSub_.subscribe(nh, "odom", 1);
			if(subscribeScan2d)
			{
				subscribedToScan2d_ = true;
				scanSub_.subscribe(nh, "scan", 1);
				SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
			}
			else if(subscribeScan3d)
			{
				subscribedToScan3d_ = true;
				scan3dSub_.subscribe(nh, "scan_cloud", 1);
				SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
			}
			else if(subscribeOdomInfo)
			{
				subscribedToOdomInfo_ = true;
				odomInfoSub_.subscribe(nh, "odom_info", 1);
				SYNC_DECL3(rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
			}
			else
			{
				SYNC_DECL2(rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
			}
		}
		else if(subscribeUserData)
		{
			userDataSub_.subscribe(nh, "user_data", 1);
			if(subscribeScan2d)
			{
				subscribedToScan2d_ = true;
				scanSub_.subscribe(nh, "scan", 1);
				SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
			}
			else if(subscribeScan3d)
			{
				subscribedToScan3d_ = true;
				scan3dSub_.subscribe(nh, "scan_cloud", 1);
				SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
			}
			else if(subscribeOdomInfo)
			{
				subscribedToOdomInfo_ = true;
				odomInfoSub_.subscribe(nh, "odom_info", 1);
				SYNC_DECL3(rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
			}
			else
			{
				SYNC_DECL2(rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
			}
		}
		else
		{
			if(subscribeScan2d)
			{
				subscribedToScan2d_ = true;
				scanSub_.subscribe(nh, "scan", 1);
				SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
			}
			else if(subscribeScan3d)
			{
				subscribedToScan3d_ = true;
				scan3dSub_.subscribe(nh, "scan_cloud", 1);
				SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
			}
			else if(subscribeOdomInfo)
			{
				subscribedToOdomInfo_ = true;
				odomInfoSub_.subscribe(nh, "odom_info", 1);
				SYNC_DECL2(rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
			}
			else
			{
				ROS_FATAL("Not supposed to be here!");
			}
		}
	}
	else
	{
		rgbdSub_ = nh.subscribe("rgbd_image", 1, &CommonDataSubscriber::rgbdCallback, this);

		subscribedTopicsMsg_ =
				uFormat("\n%s subscribed to:\n   %s",
				ros::this_node::getName().c_str(),
				rgbdSub_.getTopic().c_str());
	}
}
示例#16
0
bool TreeKinematics::getPositionFk(kinematics_msgs::GetPositionFK::Request& request,
                                   kinematics_msgs::GetPositionFK::Response& response)
{
  ROS_DEBUG("getPositionFK method invoked.");
  KDL::JntArray q_in;
  double nr_of_jnts = request.robot_state.joint_state.name.size();
  q_in.resize(nr_of_jnts);

  for (unsigned int i=0; i < nr_of_jnts; ++i)
  {
    int tmp_index = getJointIndex(request.robot_state.joint_state.name[i]);
    if (tmp_index >=0)
    {
      q_in(tmp_index) = request.robot_state.joint_state.position[i];
      ROS_DEBUG("joint '%s' is now number '%d'", request.robot_state.joint_state.name[i].c_str(), tmp_index);
    }
    else
    {
      ROS_FATAL("i: %d, No joint index for %s!", i, request.robot_state.joint_state.name[i].c_str());
      ROS_FATAL("Service call aborted.");
      return false;
    }
  }

  response.pose_stamped.resize(request.fk_link_names.size());
  response.fk_link_names.resize(request.fk_link_names.size());
  KDL::Frame p_out;
  geometry_msgs::PoseStamped pose;
  tf::Stamped<tf::Pose> tf_pose;
  for (unsigned int i=0; i < request.fk_link_names.size(); i++)
  {
    ROS_DEBUG("End effector name: %s",request.fk_link_names[i].c_str());
    int fk_ret = fk_solver_->JntToCart(q_in, p_out, request.fk_link_names[i]);
    if (fk_ret >= 0)
    {
      tf_pose.frame_id_ = tree_root_name_;
      tf_pose.stamp_ = ros::Time::now();
      tf::PoseKDLToTF(p_out, tf_pose);
      try
      {
        tf_listener_.transformPose(request.header.frame_id, tf_pose, tf_pose);
      }
      catch (tf::TransformException const &ex)
      {
        ROS_FATAL("Could not transform FK pose to frame: %s",request.header.frame_id.c_str());
        response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
        return false;
      }
      tf::poseStampedTFToMsg(tf_pose, pose);
      response.pose_stamped[i] = pose;
      response.fk_link_names[i] = request.fk_link_names[i];
      response.error_code.val = response.error_code.SUCCESS;
    }
    else
    {
      ROS_WARN("A FK solution could not be found for %s (error code = %d)",
      request.fk_link_names[i].c_str(), fk_ret);
      response.error_code.val = response.error_code.NO_FK_SOLUTION;
    }
  }

  return true;
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosP3D::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
  // Get the world name.
  this->world_ = _parent->GetWorld();
  this->model_ = _parent;

  // load parameters
  this->robot_namespace_ = "";
  if (_sdf->HasElement("robotNamespace"))
    this->robot_namespace_ =
      _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";

  if (!_sdf->HasElement("bodyName"))
  {
    ROS_FATAL("p3d plugin missing <bodyName>, cannot proceed");
    return;
  }
  else
    this->link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();

  this->link_ = _parent->GetLink(this->link_name_);
  if (!this->link_)
  {
    ROS_FATAL("gazebo_ros_p3d plugin error: bodyName: %s does not exist\n",
      this->link_name_.c_str());
    return;
  }

  if (!_sdf->HasElement("topicName"))
  {
    ROS_FATAL("p3d plugin missing <topicName>, cannot proceed");
    return;
  }
  else
    this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();

  if (!_sdf->HasElement("frameName"))
  {
    ROS_DEBUG("p3d plugin missing <frameName>, defaults to world");
    this->frame_name_ = "world";
  }
  else
    this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();

  if (!_sdf->HasElement("xyzOffset"))
  {
    ROS_DEBUG("p3d plugin missing <xyzOffset>, defaults to 0s");
    this->offset_.pos = math::Vector3(0, 0, 0);
  }
  else
    this->offset_.pos = _sdf->GetElement("xyzOffset")->Get<math::Vector3>();

  if (!_sdf->HasElement("rpyOffset"))
  {
    ROS_DEBUG("p3d plugin missing <rpyOffset>, defaults to 0s");
    this->offset_.rot = math::Vector3(0, 0, 0);
  }
  else
    this->offset_.rot = _sdf->GetElement("rpyOffset")->Get<math::Vector3>();

  if (!_sdf->HasElement("gaussianNoise"))
  {
    ROS_DEBUG("p3d plugin missing <gaussianNoise>, defaults to 0.0");
    this->gaussian_noise_ = 0;
  }
  else
    this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->Get<double>();

  if (!_sdf->HasElement("updateRate"))
  {
    ROS_DEBUG("p3d plugin missing <updateRate>, defaults to 0.0"
             " (as fast as possible)");
    this->update_rate_ = 0;
  }
  else
    this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();

  // Make sure the ROS node for Gazebo has already been initialized
  if (!ros::isInitialized())
  {
    ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
      << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
    return;
  }

  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);

  // publish multi queue
  this->pmq.startServiceThread();

  // resolve tf prefix
  std::string prefix;
  this->rosnode_->getParam(std::string("tf_prefix"), prefix);
  this->tf_frame_name_ = tf::resolve(prefix, this->frame_name_);

  if (this->topic_name_ != "")
  {
    this->pub_Queue = this->pmq.addPub<nav_msgs::Odometry>();
    this->pub_ =
      this->rosnode_->advertise<nav_msgs::Odometry>(this->topic_name_, 1);
  }

  this->last_time_ = this->world_->GetSimTime();
  // initialize body
  this->last_vpos_ = this->link_->GetWorldLinearVel();
  this->last_veul_ = this->link_->GetWorldAngularVel();
  this->apos_ = 0;
  this->aeul_ = 0;

  // if frameName specified is "/world", "world", "/map" or "map" report
  // back inertial values in the gazebo world
  if (this->frame_name_ != "/world" &&
      this->frame_name_ != "world" &&
      this->frame_name_ != "/map" &&
      this->frame_name_ != "map")
  {
    this->reference_link_ = this->model_->GetLink(this->frame_name_);
    if (!this->reference_link_)
    {
      ROS_ERROR("gazebo_ros_p3d plugin: frameName: %s does not exist, will"
                " not publish pose\n", this->frame_name_.c_str());
      return;
    }
  }

  // init reference frame state
  if (this->reference_link_)
  {
    ROS_DEBUG("got body %s", this->reference_link_->GetName().c_str());
    this->frame_apos_ = 0;
    this->frame_aeul_ = 0;
    this->last_frame_vpos_ = this->reference_link_->GetWorldLinearVel();
    this->last_frame_veul_ = this->reference_link_->GetWorldAngularVel();
  }


  // start custom queue for p3d
  this->callback_queue_thread_ = boost::thread(
    boost::bind(&GazeboRosP3D::P3DQueueThread, this));

  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboRosP3D::UpdateChild, this));
}
示例#18
0
bool TreeKinematics::getPositionIk(tree_kinematics::get_tree_position_ik::Request &request,
                                   tree_kinematics::get_tree_position_ik::Response &response)
{
  ik_srv_duration_ = ros::Time::now().toSec();
  ROS_DEBUG("getPositionIK method invoked.");

  // extract current joint positions from the request
  KDL::JntArray q, q_desi;
  double nr_of_jnts = request.pos_ik_request[0].ik_seed_state.joint_state.name.size();
  q.resize(nr_of_jnts);
  q_desi.resize(nr_of_jnts);
  for (unsigned int i=0; i < nr_of_jnts; ++i)
  {
    int tmp_index = getJointIndex(request.pos_ik_request[0].ik_seed_state.joint_state.name[i]);
    if (tmp_index >=0)
    {
      q(tmp_index) = request.pos_ik_request[0].ik_seed_state.joint_state.position[i];
      ROS_DEBUG("joint '%s' is now number '%d'", request.pos_ik_request[0].ik_seed_state.joint_state.name[i].c_str(),
                                                tmp_index);
    }
    else
    {
      ROS_FATAL("i: %d, No joint index for %s!",i,request.pos_ik_request[0].ik_seed_state.joint_state.name[i].c_str());
      ROS_FATAL("Service call aborted.");
      return false;
    }
  }

  // convert pose messages into transforms from the root frame to the given poses and further into KDL::Frames
  geometry_msgs::PoseStamped pose_msg_in;
  geometry_msgs::PoseStamped pose_msg_transformed;
  tf::Stamped<tf::Pose> transform;
  tf::Stamped<tf::Pose> transform_root;
  KDL::Frames desired_poses;
  KDL::Frame desired_pose;
  for(unsigned int i = 0; i < request.pos_ik_request.size(); ++i)
  {
    pose_msg_in = request.pos_ik_request[i].pose_stamped;
    try
    {
      tf_listener_.waitForTransform(tree_root_name_, pose_msg_in.header.frame_id, pose_msg_in.header.stamp,
      ros::Duration(0.1));
      tf_listener_.transformPose(tree_root_name_, pose_msg_in, pose_msg_transformed);
    }
    catch (tf::TransformException const &ex)
    {
      ROS_FATAL("%s",ex.what());
      ROS_FATAL("Could not transform IK pose from '%s' to frame '%s'", tree_root_name_.c_str(),
      pose_msg_in.header.frame_id.c_str());
      response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
      return false;
    }
    tf::PoseMsgToKDL(pose_msg_transformed.pose, desired_pose);
    desired_poses[request.pos_ik_request[i].ik_link_name] = desired_pose;
  }

  // use the solver to compute desired joint positions
  ik_duration_ = ros::Time::now().toSec();
  int ik_ret = ik_pos_solver_->CartToJnt_it(q, desired_poses, q_desi); // NOTE: Before it was CartToJnt (without the _it). What's the difference?
  ik_duration_ = ros::Time::now().toSec() - ik_duration_;
  ik_duration_median_ = ((ik_duration_median_ * (loop_count_ - 1)) + ik_duration_) / loop_count_;

  // insert the solver's result into the service response
  if (ik_ret >= 0 || ik_ret == -3)
  {
    response.solution.joint_state.name = info_.joint_names;
    response.solution.joint_state.position.resize(nr_of_jnts_);
    response.solution.joint_state.velocity.resize(nr_of_jnts_);
    for (unsigned int i=0; i < nr_of_jnts_; ++i)
    {
      response.solution.joint_state.position[i] = q_desi(i);
      response.solution.joint_state.velocity[i] = (q_desi(i) - q(i)) * srv_call_frequency_;
      ROS_DEBUG("IK Solution: %s %d: pos = %f, vel = %f",response.solution.joint_state.name[i].c_str(), i,
      response.solution.joint_state.position[i], response.solution.joint_state.velocity[i]);
    }
    response.error_code.val = response.error_code.SUCCESS;

    ik_srv_duration_ = ros::Time::now().toSec() - ik_srv_duration_;
    ik_srv_duration_median_ = ((ik_srv_duration_median_ * (loop_count_ - 1)) + ik_srv_duration_) / loop_count_;
    loop_count_++;

    ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_srv_duration %f and median %f", ik_srv_duration_,
    ik_srv_duration_median_);
    ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_duration %f and median %f", ik_duration_, ik_duration_median_);

    if (ik_ret == -3)
    {
      ROS_WARN_THROTTLE(1.0, "Maximum iterations reached! (error code = %d)", ik_ret);
    }
  }
  else
  {
    ROS_WARN("An IK solution could not be found (error code = %d)", ik_ret);
    response.error_code.val = response.error_code.NO_IK_SOLUTION;

    ik_srv_duration_ = ros::Time::now().toSec() - ik_srv_duration_;
    ik_srv_duration_median_ = ((ik_srv_duration_median_ * (loop_count_ - 1)) + ik_srv_duration_) / loop_count_;
    loop_count_++;

    ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_srv_duration %f and median %f", ik_srv_duration_,
    ik_srv_duration_median_);
    ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_duration %f and median %f", ik_duration_, ik_duration_median_);
  }
  return true;
}
示例#19
0
int main(int argc, char** argv)
{
	ROS_INFO("Started fullbody_teleop.");
	ros::init(argc, argv, "fullbody_teleop");

	ros::NodeHandle nh;

	std::string robotDescription;
	if (!nh.getParam("/robot_description", robotDescription))
	{
		ROS_FATAL("Parameter for robot description not provided");
	}
	huboModel.initString(robotDescription);

	//ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);

	gIntServer.reset( new interactive_markers::InteractiveMarkerServer("joint_controls","",false) );

	for (auto jointPair : huboModel.joints_)
	{
		boost::shared_ptr<urdf::Joint> joint = jointPair.second;
		if (joint->name[1] == 'F') { continue; }
		if (joint->name== "TSY") { continue; }
		makeJointMarker(joint->name);
	}
	std::cerr << "\nURDF size: " << huboModel.joints_.size() << std::endl;

	ros::Duration(0.1).sleep();

	for (int i = 0; i < HUBO_JOINT_COUNT; i++)
	{
		if (DRCHUBO_URDF_JOINT_NAMES[i] != "")
		{
			planState.name.push_back(DRCHUBO_URDF_JOINT_NAMES[i]);
			planState.position.push_back(0);
			planState.velocity.push_back(0);
			planState.effort.push_back(0);
		}
	}

	for (int side = 0; side < 2; side++)
		for (int i = 1; i <= 3; i++)
			for (int j = 1; j <= 3; j++)
			{
				std::string sideStr = (side == 0) ? "R" : "L";
				planState.name.push_back(sideStr + "F" + std::to_string(i) + std::to_string(j));
				planState.position.push_back(0);
				planState.velocity.push_back(0);
				planState.effort.push_back(0);
			}

	makeSaveButton();
	gIntServer->applyChanges();

	gJoySubscriber = nh.subscribe("joy_in", 1, &joyCallback);
	gIKinClient = nh.serviceClient<moveit_msgs::GetPositionIK>("/hubo/kinematics/ik_service");
	gFKinClient = nh.serviceClient<moveit_msgs::GetPositionFK>("/hubo/kinematics/fk_service");
	gStatePublisher = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
	gRPosePublisher = nh.advertise<geometry_msgs::PoseStamped>("rh_pose", 1);
	gTextPublisher = nh.advertise<std_msgs::String>("text_out", 1);

	gTimer = nh.createTimer(ros::Duration(1), &timerCallback);
	gTimer.start();

	ros::spin();

	gIntServer.reset();
	return 0;
}
示例#20
0
ImuFilter::ImuFilter(ros::NodeHandle nh, ros::NodeHandle nh_private):
  nh_(nh), 
  nh_private_(nh_private),
  initialized_(false),
  q0(1.0), q1(0.0), q2(0.0), q3(0.0)
{
  ROS_INFO ("Starting ImuFilter");

  // **** get paramters

  if (!nh_private_.getParam ("gain", gain_))
   gain_ = 0.1;
  if (!nh_private_.getParam ("use_mag", use_mag_))
   use_mag_ = true;
  if (!nh_private_.getParam ("publish_tf", publish_tf_))
   publish_tf_ = true;
  if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
   fixed_frame_ = "odom";
  if (!nh_private_.getParam ("constant_dt", constant_dt_))
    constant_dt_ = 0.0;

  // check for illegal constant_dt values
  if (constant_dt_ < 0.0)
  {
    ROS_FATAL("constant_dt parameter is %f, must be >= 0.0. Setting to 0.0", constant_dt_);
    constant_dt_ = 0.0;
  }
  
  // if constant_dt_ is 0.0 (default), use IMU timestamp to determine dt
  // otherwise, it will be constant
  if (constant_dt_ == 0.0)
    ROS_INFO("Using dt computed from message headers");
  else
    ROS_INFO("Using constant dt of %f sec", constant_dt_);

  // **** register dynamic reconfigure
  
  FilterConfigServer::CallbackType f = boost::bind(&ImuFilter::reconfigCallback, this, _1, _2);
  config_server_.setCallback(f);
  
  // **** register publishers

  imu_publisher_ = nh_.advertise<sensor_msgs::Imu>(
    "imu/data", 5);

  // **** register subscribers

  // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
  int queue_size = 5;

  imu_subscriber_.reset(new ImuSubscriber(
    nh_, "imu/data_raw", queue_size));

  if (use_mag_)
  {
    mag_subscriber_.reset(new MagSubscriber(
      nh_, "imu/mag", queue_size));

    sync_.reset(new Synchronizer(
      SyncPolicy(queue_size), *imu_subscriber_, *mag_subscriber_));
    sync_->registerCallback(boost::bind(&ImuFilter::imuMagCallback, this, _1, _2));
  }
  else
  {
    imu_subscriber_->registerCallback(&ImuFilter::imuCallback, this);
  }
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosMagnetic::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  world = _model->GetWorld();

  // load parameters
  if (_sdf->HasElement("robotNamespace"))
    namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString();
  else
    namespace_.clear();

  if (!_sdf->HasElement("topicName"))
    topic_ = "magnetic";
  else
    topic_ = _sdf->GetElement("topicName")->Get<std::string>();


  if (_sdf->HasElement("bodyName"))
  {
    link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString();
    link = _model->GetLink(link_name_);
  }
  else
  {
    link = _model->GetLink();
    link_name_ = link->GetName();
  }

  if (!link)
  {
    ROS_FATAL("GazeboRosMagnetic plugin error: bodyName: %s does not exist\n", link_name_.c_str());
    return;
  }

  // default parameters
  frame_id_ = link_name_;
  magnitude_ = DEFAULT_MAGNITUDE;
  reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0;
  declination_ = DEFAULT_DECLINATION * M_PI/180.0;
  inclination_ = DEFAULT_INCLINATION * M_PI/180.0;

  if (_sdf->HasElement("frameId"))
    frame_id_ = _sdf->GetElement("frameId")->GetValue()->GetAsString();

  if (_sdf->HasElement("magnitude"))
    _sdf->GetElement("magnitude")->GetValue()->Get(magnitude_);

  if (_sdf->HasElement("referenceHeading"))
    if (_sdf->GetElement("referenceHeading")->GetValue()->Get(reference_heading_))
      reference_heading_ *= M_PI/180.0;

  if (_sdf->HasElement("declination"))
    if (_sdf->GetElement("declination")->GetValue()->Get(declination_))
      declination_ *= M_PI/180.0;

  if (_sdf->HasElement("inclination"))
    if (_sdf->GetElement("inclination")->GetValue()->Get(inclination_))
      inclination_ *= M_PI/180.0;

  // Note: Gazebo uses NorthWestUp coordinate system, heading and declination are compass headings
  magnetic_field_.header.frame_id = frame_id_;
  magnetic_field_world_.x = magnitude_ *  cos(inclination_) * cos(reference_heading_ - declination_);
  magnetic_field_world_.y = magnitude_ *  cos(inclination_) * sin(reference_heading_ - declination_);
  magnetic_field_world_.z = magnitude_ * -sin(inclination_);

  sensor_model_.Load(_sdf);

  // start ros node
  if (!ros::isInitialized())
  {
    int argc = 0;
    char** argv = NULL;
    ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
  }

  node_handle_ = new ros::NodeHandle(namespace_);
  publisher_ = node_handle_->advertise<geometry_msgs::Vector3Stamped>(topic_, 1);

  // setup dynamic_reconfigure server
  dynamic_reconfigure_server_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, topic_)));
  dynamic_reconfigure_server_->setCallback(boost::bind(&SensorModel3::dynamicReconfigureCallback, &sensor_model_, _1, _2));

  Reset();

  // connect Update function
  updateTimer.Load(world, _sdf);
  updateConnection = updateTimer.Connect(boost::bind(&GazeboRosMagnetic::Update, this));
}
void cb_points(const visualization_msgs::MarkerConstPtr& data)
{
    bool success = false;
    double x = data->pose.position.x;
    double y = data->pose.position.y ;
    double z = data->pose.position.z;

    ROS_INFO("Points received! x: %f y: %f z: %f",x,y,z);



    moveit::planning_interface::MoveGroup group("manipulator");
    group.setPlanningTime(45.0);
    group.setNumPlanningAttempts(10);
    group.allowReplanning(true);
    group.clearPathConstraints();

    co.header.stamp = ros::Time::now();
    co.header.frame_id = "base_link";
    co.id = "testbox";
    co.primitives.resize(1);
    co.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER;
    co.primitives[0].dimensions.resize(shape_tools::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::CYLINDER>::value);
    co.primitives[0].dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = 0.04;
    co.primitives[0].dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = 0.14;

    //co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.14;
    co.primitive_poses.resize(1);
    co.primitive_poses[0].position.x = x;
    co.primitive_poses[0].position.y = y;
    co.primitive_poses[0].position.z = z;
    co.primitive_poses[0].orientation.w = 1.0;

    aco.object = co;
    aco.link_name = "katana_gripper_tool_frame";

    add_collision_object();
    ros::WallDuration(5.0).sleep();

    ROS_INFO("Generating Grasps");
    std::vector<moveit_msgs::Grasp> grasps = generate_grasps(x, y, z);

    success = group.pick("testbox",grasps);
    if (success)
    {
        ROS_INFO("Pick was successful.");
    }
    else
    {
        ROS_FATAL("Pick failed.");
        //return EXIT_FAILURE;
    }

    ros::WallDuration(3.0).sleep();


    if(success)
    {
        std::vector<moveit_msgs::PlaceLocation> places = generate_places(x, -y, z);
        success = group.place("testbox",places);
        if (success)
        {

            ROS_INFO("Place was successful.");
            geometry_msgs::PoseStamped po = group.getCurrentPose();
            ROS_INFO("x: %f y: %f z: %f",po.pose.position.x,po.pose.position.y,po.pose.position.z);
            ROS_INFO("w: %f, x: %f y: %f z: %f",po.pose.orientation.w, po.pose.orientation.x, po.pose.orientation.y, po.pose.orientation.z);
        }
        else
        {
            ROS_FATAL("Place failed.");
            //return EXIT_FAILURE;
        }
    }
    group.setStartState(*group.getCurrentState());
    remove_collision_object();

    group.setStartState(*group.getCurrentState());
    group.setNamedTarget("home");
    group.move();
    geometry_msgs::PoseStamped po = group.getCurrentPose();
    ROS_INFO("x: %f y: %f z: %f",po.pose.position.x,po.pose.position.y,po.pose.position.z);
    ROS_INFO("w: %f, x: %f y: %f z: %f",po.pose.orientation.w, po.pose.orientation.x, po.pose.orientation.y, po.pose.orientation.z);


}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosMagnetic::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  world = _model->GetWorld();

  // load parameters
  if (!_sdf->HasElement("robotNamespace"))
    namespace_.clear();
  else
    namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";

  if (!_sdf->HasElement("topicName"))
    topic_ = "magnetic";
  else
    topic_ = _sdf->GetElement("topicName")->GetValueString();

  if (!_sdf->HasElement("bodyName"))
  {
    link = _model->GetLink();
    link_name_ = link->GetName();
  }
  else {
    link_name_ = _sdf->GetElement("bodyName")->GetValueString();
    link = boost::shared_dynamic_cast<physics::Link>(world->GetEntity(link_name_));
  }

  if (!link)
  {
    ROS_FATAL("GazeboRosMagnetic plugin error: bodyName: %s does not exist\n", link_name_.c_str());
    return;
  }

  double update_rate = 0.0;
  if (_sdf->HasElement("updateRate")) update_rate = _sdf->GetElement("updateRate")->GetValueDouble();
  update_period = update_rate > 0.0 ? 1.0/update_rate : 0.0;

  if (!_sdf->HasElement("frameId"))
    frame_id_ = link_name_;
  else
    frame_id_ = _sdf->GetElement("frameId")->GetValueString();

  if (!_sdf->HasElement("magnitude"))
    magnitude_ = DEFAULT_MAGNITUDE;
  else
    magnitude_ = _sdf->GetElement("magnitude")->GetValueDouble();

  if (!_sdf->HasElement("referenceHeading"))
    reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0;
  else
    reference_heading_ = _sdf->GetElement("referenceHeading")->GetValueDouble() * M_PI/180.0;

  if (!_sdf->HasElement("declination"))
    declination_ = DEFAULT_DECLINATION * M_PI/180.0;
  else
    declination_ = _sdf->GetElement("declination")->GetValueDouble() * M_PI/180.0;

  if (!_sdf->HasElement("inclination"))
    inclination_ = DEFAULT_INCLINATION * M_PI/180.0;
  else
    inclination_ = _sdf->GetElement("inclination")->GetValueDouble() * M_PI/180.0;

  // Note: Gazebo uses NorthWestUp coordinate system, heading and declination are compass headings
  magnetic_field_.header.frame_id = frame_id_;
  magnetic_field_world_.x = magnitude_ *  cos(inclination_) * cos(reference_heading_ - declination_);
  magnetic_field_world_.y = magnitude_ *  sin(reference_heading_ - declination_);
  magnetic_field_world_.z = magnitude_ * -sin(inclination_) * cos(reference_heading_ - declination_);

  sensor_model_.Load(_sdf);

  // start ros node
  if (!ros::isInitialized())
  {
    int argc = 0;
    char** argv = NULL;
    ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
  }

  node_handle_ = new ros::NodeHandle(namespace_);
  publisher_ = node_handle_->advertise<geometry_msgs::Vector3Stamped>(topic_, 1);

  Reset();

  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  updateConnection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboRosMagnetic::Update, this));
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboTruck::Update()
{
  boost::mutex::scoped_lock scoped_lock(lock);

  if ( terminated_ )
    {
      return;
    }

  // std::cerr << link_->GetWorldPose() << std::endl;
  common::Time current_time = world_->GetSimTime();
  double delta_time = (current_time-last_time_).Double();
  double theta = acos(CIRCLE_RADIUS/(CIRCLE_DISTANCE/2));
  double x, y, yaw;
  double l1 = CIRCLE_DISTANCE*sin(theta);
  double l2 = 2*CIRCLE_RADIUS*(M_PI-theta);
  double all_l = l1 + l2 + l1 + l2;

  // static const float VELOCITY = 4.16667;  //  4.16667 m/sec = 15 km/h, 2.77778 m/sec = 10 km/h, 1.38889 = 5 km/h
  if ( current_time.Double() < 6*60 )
    {
      traversed_ += 4.16667 * delta_time;
    }
  else if ( current_time.Double() < 12*60 )
    {
      traversed_ += 2.77778 * delta_time;
    }
  else if ( current_time.Double() < 20*60 )
    {
      traversed_ += 1.38889 * delta_time;
    }
  else
    {
      ROS_FATAL("Time's up, Your challenge was over");
      terminated_ = true;
    }
  double l = fmod(traversed_, all_l);
  ROS_DEBUG_STREAM("time: " << current_time.Double() << ", traversed: " << traversed_);

  if ( l < l1  )
    {
      x =  (l - l1/2)*sin(theta);
      y = -(l - l1/2)*cos(theta);
      yaw = theta-M_PI/2;
    }
  else if ( l < l1 + l2 )
    {
      l = l - l1;
      x = -CIRCLE_RADIUS * cos(l/l2*(2*M_PI-theta*2)+theta) + CIRCLE_DISTANCE/2;
      y = -CIRCLE_RADIUS * sin(l/l2*(2*M_PI-theta*2)+theta);
      yaw = (l/l2*(2*M_PI-theta*2)+theta)-M_PI/2;
    }
  else if ( l < l1 + l2 + l1 )
    {
      l = l - (l1 + l2);
      x = -(l - l1/2)*sin(theta);
      y = -(l - l1/2)*cos(theta);
      yaw =-M_PI/2-theta;
    }
  else
    {
      l = l - (l1 + l2 + l1);
      x =  CIRCLE_RADIUS * cos(l/l2*(2*M_PI-theta*2)+theta) - CIRCLE_DISTANCE/2;
      y = -CIRCLE_RADIUS * sin(l/l2*(2*M_PI-theta*2)+theta);
      yaw = -(l/l2*(2*M_PI-theta*2)+theta)-M_PI/2;
    }
  model_->SetLinkWorldPose(math::Pose(x, y, 0.595, 0, 0, yaw), link_);
  last_time_ = world_->GetSimTime();

  // check score
  // void Entity::GetNearestEntityBelow(double &_distBelow,  std::string &_entityName)

  gazebo::physics::RayShapePtr rayShape = boost::dynamic_pointer_cast<gazebo::physics::RayShape>(
    world_->GetPhysicsEngine()->CreateShape("ray", gazebo::physics::CollisionPtr()));

  double distAbove;
  std::string entityName;
  math::Box box = model_->GetLink("heliport")->GetCollisionBoundingBox();
  math::Vector3 start = model_->GetLink("heliport")->GetWorldPose().pos;
  math::Vector3 end = start;
  start.z = box.max.z + 0.00001;
  end.z += 1000;
  rayShape->SetPoints(start, end);
  rayShape->GetIntersection(distAbove, entityName);
  distAbove -= 0.00001;

  // publish remain time
  std::stringstream ss;
  std_msgs::String msg_time;
  ss << 20*60 - current_time.Double();
  msg_time.data = "remain time:" + ss.str();
  pub_time_.publish(msg_time);
  if ( entityName != "" && distAbove < 1.0 )
    {
      std_msgs::String msg_score, msg_time;
      msg_score.data = "Mission Completed";
      ROS_INFO_STREAM("Remaining time is " << msg_time.data << "[sec], Score is " << msg_score.data);
      pub_score_.publish(msg_score);
      terminated_ = true;
    }
}
示例#25
0
  UsbCamNode() :
    node_("~")
  {
    image_transport::ImageTransport it(node_);
    std::string topicName;
    node_.param("topicName",topicName,std::string("image_raw"));
    image_pub_ = it.advertiseCamera(topicName, 1);

    node_.param("video_device", video_device_name_, std::string("/dev/video0"));
    node_.param("io_method", io_method_name_, std::string("mmap")); // possible values: mmap, read, userptr
    node_.param("image_width", image_width_, 640);
    node_.param("image_height", image_height_, 480);
    node_.param("framerate", framerate_, 30);
    node_.param("pixel_format", pixel_format_name_, std::string("mjpeg")); // possible values: yuyv, uyvy, mjpeg
    node_.param("autofocus", autofocus_, false); // enable/disable autofocus


    node_.param("camera_frame_id", img_.header.frame_id, std::string("head_camera"));
    node_.param("camera_name", camera_name_, std::string("head_camera"));
    node_.param("camera_info_url", camera_info_url_, std::string(""));
    cinfo_.reset(new camera_info_manager::CameraInfoManager(node_, camera_name_, camera_info_url_));

    ROS_INFO("Camera name: %s", camera_name_.c_str());
    ROS_INFO("Camera info url: %s", camera_info_url_.c_str());
    ROS_INFO("usb_cam video_device set to [%s]\n", video_device_name_.c_str());
    ROS_INFO("usb_cam io_method set to [%s]\n", io_method_name_.c_str());
    ROS_INFO("usb_cam image_width set to [%d]\n", image_width_);
    ROS_INFO("usb_cam image_height set to [%d]\n", image_height_);
    ROS_INFO("usb_cam pixel_format set to [%s]\n", pixel_format_name_.c_str());
    ROS_INFO("usb_cam auto_focus set to [%d]\n", autofocus_);

    usb_cam_io_method io_method;
    if(io_method_name_ == "mmap")
      io_method = IO_METHOD_MMAP;
    else if(io_method_name_ == "read")
      io_method = IO_METHOD_READ;
    else if(io_method_name_ == "userptr")
      io_method = IO_METHOD_USERPTR;
    else {
      ROS_FATAL("Unknown io method.");
      node_.shutdown();
      return;
    }

    usb_cam_pixel_format pixel_format;
    if(pixel_format_name_ == "yuyv")
      pixel_format = PIXEL_FORMAT_YUYV;
    else if(pixel_format_name_ == "uyvy")
      pixel_format = PIXEL_FORMAT_UYVY;
    else if(pixel_format_name_ == "mjpeg") {
      pixel_format = PIXEL_FORMAT_MJPEG;
    }
    else {
      ROS_FATAL("Unknown pixel format.");
      node_.shutdown();
      return;
    }

    camera_image_ = usb_cam_camera_start(video_device_name_.c_str(),
                                         io_method,
                                         pixel_format,
                                         image_width_,
                                         image_height_,
                                         framerate_);

    if(autofocus_) {
      usb_cam_camera_set_auto_focus(1);
    }

    next_time_ = ros::Time::now();
    count_ = 0;
  }
示例#26
0
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosIMU::LoadThread()
{
  // load parameters
  this->robot_namespace_ = "";
  if (this->sdf->HasElement("robotNamespace"))
    this->robot_namespace_ = this->sdf->Get<std::string>("robotNamespace") + "/";

  if (!this->sdf->HasElement("serviceName"))
  {
    ROS_INFO("imu plugin missing <serviceName>, defaults to /default_imu");
    this->service_name_ = "/default_imu";
  }
  else
    this->service_name_ = this->sdf->Get<std::string>("serviceName");

  if (!this->sdf->HasElement("topicName"))
  {
    ROS_INFO("imu plugin missing <topicName>, defaults to /default_imu");
    this->topic_name_ = "/default_imu";
  }
  else
    this->topic_name_ = this->sdf->Get<std::string>("topicName");

  if (!this->sdf->HasElement("gaussianNoise"))
  {
    ROS_INFO("imu plugin missing <gaussianNoise>, defaults to 0.0");
    this->gaussian_noise_ = 0.0;
  }
  else
    this->gaussian_noise_ = this->sdf->Get<double>("gaussianNoise");

  if (!this->sdf->HasElement("bodyName"))
  {
    ROS_FATAL("imu plugin missing <bodyName>, cannot proceed");
    return;
  }
  else
    this->link_name_ = this->sdf->Get<std::string>("bodyName");

  if (!this->sdf->HasElement("xyzOffset"))
  {
    ROS_INFO("imu plugin missing <xyzOffset>, defaults to 0s");
    this->offset_.pos = math::Vector3(0, 0, 0);
  }
  else
    this->offset_.pos = this->sdf->Get<math::Vector3>("xyzOffset");

  if (!this->sdf->HasElement("rpyOffset"))
  {
    ROS_INFO("imu plugin missing <rpyOffset>, defaults to 0s");
    this->offset_.rot = math::Vector3(0, 0, 0);
  }
  else
    this->offset_.rot = this->sdf->Get<math::Vector3>("rpyOffset");
  
  if (!this->sdf->HasElement("updateRate"))
  {
    ROS_DEBUG("imu plugin missing <updateRate>, defaults to 0.0"
             " (as fast as possible)");
    this->update_rate_ = 0.0;
  }
  else
    this->update_rate_ = this->sdf->GetElement("updateRate")->Get<double>();


  // Make sure the ROS node for Gazebo has already been initialized
  if (!ros::isInitialized())
  {
    ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
      << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
    return;
  }

  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);

  // publish multi queue
  this->pmq.startServiceThread();

  // assert that the body by link_name_ exists
  this->link = boost::dynamic_pointer_cast<physics::Link>(
    this->world_->GetEntity(this->link_name_));
  if (!this->link)
  {
    ROS_FATAL("gazebo_ros_imu plugin error: bodyName: %s does not exist\n",
      this->link_name_.c_str());
    return;
  }

  // if topic name specified as empty, do not publish
  if (this->topic_name_ != "")
  {
    this->pub_Queue = this->pmq.addPub<sensor_msgs::Imu>();
    this->pub_ = this->rosnode_->advertise<sensor_msgs::Imu>(
      this->topic_name_, 1);

    // advertise services on the custom queue
    ros::AdvertiseServiceOptions aso =
      ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
      this->service_name_, boost::bind(&GazeboRosIMU::ServiceCallback,
      this, _1, _2), ros::VoidPtr(), &this->imu_queue_);
    this->srv_ = this->rosnode_->advertiseService(aso);
  }

  // Initialize the controller
  this->last_time_ = this->world_->GetSimTime();

  // this->initial_pose_ = this->link->GetPose();
  this->last_vpos_ = this->link->GetWorldLinearVel();
  this->last_veul_ = this->link->GetWorldAngularVel();
  this->apos_ = 0;
  this->aeul_ = 0;

  // start custom queue for imu
  this->callback_queue_thread_ =
    boost::thread(boost::bind(&GazeboRosIMU::IMUQueueThread, this));


  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboRosIMU::UpdateChild, this));
}
示例#27
0
  void callback(const sensor_msgs::JointState::ConstPtr& jointStatePtr)
  {
    ROS_INFO_STREAM("I'm in the callback");

    float L1 = 0.3;
    float L2 = 0.31;

    float theta1_L, theta1_R, theta2_L, theta2_R;
    float mPError1, mPError2;

    float r = sqrt(pow(x_estimate, 2)+pow(y_estimate, 2));
    float alpha = acosf((powf(L1,2)+powf(L2,2)-powf(r,2))/(2.0*L1*L2));
    float beta = acosf((powf(L1,2)-powf(L2,2)+powf(r,2))/(2.0*L1*r));

    theta2_L = M_PI + alpha;
    theta2_R = M_PI - alpha;

    theta1_L = atan2f(y_estimate, x_estimate) + beta;
    theta1_R = atan2f(y_estimate, x_estimate) - beta;

    theta1_L = wrapAngle(theta1_L);
    theta1_R = wrapAngle(theta1_R);
    theta2_L = wrapAngle(theta2_L);
    theta2_R = wrapAngle(theta2_R);

    sensor_msgs::JointState output;
    output.header.stamp = ros::Time::now();
    output.header.frame_id = "";

    if (direction == "left")
    {

        mPError1 = theta1_L-jointStatePtr->position[0];
        //mIError1 += theta1_L-jointStatePtr->position[0];

        mPError2 = theta2_L-jointStatePtr->position[2];
        //mIError2 += theta2_L-jointStatePtr->position[2];
    }

    else if (direction == "right")
    {

        mPError1 = theta1_R-jointStatePtr->position[0];
        mPError2 = theta2_R-jointStatePtr->position[2];
    }

    else
        ROS_FATAL("COMMAND WAS NEITHER LEFT NOR RIGHT!!!");


    ROS_INFO("******************************GOING FOR IT!!*****************************************\n");
    ROS_INFO("x_board=%f, y_board=%f \n", x_estimate, y_estimate);
    ROS_INFO("r = %f, alpha = %f, beta = %f\n", r, alpha, beta);
    ROS_INFO("theta1_L = %f, theta1_R = %f, theta2_L = %f, theta2_R = %f\n", theta1_L, theta1_R, theta2_L, theta2_R);
    ROS_INFO("Joint Positions: (%f, %f, %f)\n", jointStatePtr->position[0], jointStatePtr->position[1], jointStatePtr->position[2]);
    ROS_INFO("P Error_1 = %f, P Error_2 = %f\n", mPError1, mPError2);
    ROS_INFO("***********************************************************************\n");
    //ROS_INFO_STREAM("Back in main, global counter is: " << globalcounter);

    float gain=0.2;


    float command1 = jointStatePtr->position[0]+gain*mPError1;
    float command2 = jointStatePtr->position[2]+gain*mPError2;

    ROS_INFO_STREAM("Command to motor 21: " << command1 << " Command to motor 23: " << command2);


    //std::cout << "commanding: " << x_estimate << ", " << y_estimate << std::endl;
    if ((finalMoveFlag == 1) && (idleFlag ==0))
    {
        ROS_INFO("finalMoveFlag is 1");
        output.position.push_back(jointStatePtr->position[0]+mPError1);
        output.position.push_back(1.0);
        output.position.push_back(jointStatePtr->position[2]+mPError2);
    }

    else if (idleFlag == 0)
    {
        output.position.push_back(command1);
        output.position.push_back(1.0);
        output.position.push_back(command2);
    }

    else
    {
        ROS_INFO("Idling the motors");
        output.position.push_back(NAN);
        output.position.push_back(1.0);
        output.position.push_back(NAN);
        output.velocity.push_back(0);
        output.velocity.push_back(0);
        output.velocity.push_back(0);

    }
    //.... do something with the input and generate the output...
    pub_.publish(output);
    ROS_INFO_STREAM("Just published!");
  }
示例#28
0
int main( int argc, char** argv ) {
    // initialize ROS
    ros::init(argc, argv, "iiwa_hw", ros::init_options::NoSigintHandler);
    
    // ros spinner
    ros::AsyncSpinner spinner(1);
    spinner.start();
    
    // custom signal handlers
    signal(SIGTERM, quitRequested);
    signal(SIGINT, quitRequested);
    signal(SIGHUP, quitRequested);
    
    // construct the lbr iiwa
    ros::NodeHandle iiwa_nh;
    IIWA_HW iiwa_robot(iiwa_nh);
    
    // configuration routines
    iiwa_robot.start();
    
    // timer variables
    struct timespec ts = {0, 0};
    ros::Time last(ts.tv_sec, ts.tv_nsec), now(ts.tv_sec, ts.tv_nsec);
    ros::Duration period(1.0);
    
    //the controller manager
    controller_manager::ControllerManager manager(&iiwa_robot, iiwa_nh);
    
    
    // run as fast as possible
    while( !g_quit ) {
        // get the time / period
        if (!clock_gettime(CLOCK_REALTIME, &ts)) {
            now.sec = ts.tv_sec;
            now.nsec = ts.tv_nsec;
            period = now - last;
            last = now;
        } else {
            ROS_FATAL("Failed to poll realtime clock!");
            break;
        } 
        
        // read current robot position
        iiwa_robot.read(period);
        
        // update the controllers
        manager.update(now, period);
        
        // send command position to the robot
        iiwa_robot.write(period);
        
        // wait for some milliseconds defined in controlFrequency
        iiwa_robot.getRate()->sleep();
        
    }
    
    std::cerr << "Stopping spinner..." << std::endl;
    spinner.stop();
    
    std::cerr << "Bye!" << std::endl;
    
    return 0;
}
示例#29
0
int main(int argc, char **argv)
{
    ros::init(argc, argv, "miniq_pid_server");

    ros::NodeHandle n;
    ros::NodeHandle pn("~");

    std::string port;
    pn.param<std::string>("port", port, "/dev/ttyUSB0");
    int baudrate;
    pn.param("baudrate", baudrate, 57600);

    ros::Publisher left_pub = n.advertise<lse_miniq_msgs::WheelVelocity>("/left_wheel_velocity", 20);
    ros::Publisher right_pub = n.advertise<lse_miniq_msgs::WheelVelocity>("/right_wheel_velocity", 20);

    if(!miniQ::openPort((char*)port.c_str(), baudrate))
    {
        ROS_FATAL("miniQ -- Failed to open serial port %s at %d baud!", port.c_str(), baudrate);
        ROS_BREAK();
    }
    ROS_INFO("miniQ PID Server -- Successfully connected to the miniQ!");

    ros::Duration(3.0).sleep();

    if(!robot.checkVersion())
    {
        ROS_FATAL("miniQ PID Server -- The firmware version of the miniQ robot is not compatible with this ROS node!");
        ROS_BREAK();
    }

    dynamic_reconfigure::Server<lse_miniq_driver::miniQConfig> server;
    dynamic_reconfigure::Server<lse_miniq_driver::miniQConfig>::CallbackType f;

    f = boost::bind(&callback, _1, _2);
    server.setCallback(f);

    turn_robot_off = false;

    ros::Rate r(2.5);
    while(n.ok())
    {
        if(!robot.updateWheelVelocities()) ROS_ERROR("miniQ PID Server -- Failed to update the miniQ wheel velocities!!!");
        else
        {
            lse_miniq_msgs::WheelVelocity right_msg;
            right_msg.reference = wheel_velocity_reference;
            right_msg.measured = robot.getRightWheelVelocity();
            right_pub.publish(right_msg);

            lse_miniq_msgs::WheelVelocity left_msg;
            left_msg.reference = wheel_velocity_reference;
            left_msg.measured = robot.getLeftWheelVelocity();
            left_pub.publish(left_msg);
        }

        if(l_s_updated)
        {
            l_s_updated = false;
            if(!robot.setPIDGains(l_s_kp, l_s_ki, l_s_kd, miniQ::LeftStartingPID)) ROS_ERROR("miniQ PID Server -- ERROR updating left starting PID!");
            ros::Duration(0.1).sleep();
        }

        if(l_r_updated)
        {
            l_r_updated = false;
            if(!robot.setPIDGains(l_r_kp, l_r_ki, l_r_kd, miniQ::LeftRunningPID)) ROS_ERROR("miniQ PID Server -- ERROR updating left running PID!");
            ros::Duration(0.1).sleep();
        }

        if(r_s_updated)
        {
            r_s_updated = false;
            if(!robot.setPIDGains(r_s_kp, r_s_ki, r_s_kd, miniQ::RightStartingPID)) ROS_ERROR("miniQ PID Server -- ERROR updating right starting PID!");
            ros::Duration(0.1).sleep();
        }

        if(r_r_updated)
        {
            r_r_updated = false;
            if(!robot.setPIDGains(r_r_kp, r_r_ki, r_r_kd, miniQ::RightRunningPID)) ROS_ERROR("miniQ PID Server -- ERROR updating right running PID!");
            ros::Duration(0.1).sleep();
        }

        if(turn_robot_off)
        {
            turn_robot_off = false;
            robot.setVelocities(0.0, 0.0);
        }
        else if(run_robot && wheel_velocity_reference_updated)
        {
            wheel_velocity_reference_updated = false;
            robot.setVelocities(wheel_velocity_reference, 0.0);
            robot.updateVelocities();
        }

        ros::spinOnce();
        r.sleep();
    }
    return 0;
}
示例#30
0
void Main::process()
{
	if(autoFaceDetection && !face_cascade.load(face_cascade_path))
	{
		ROS_FATAL("--(!)Error loading cascade detector\n"); 
		return;
	}

	while (ros::ok())
	{
		switch (state)
		{
			case INIT:
				if(newImageReceived())
				{
					if(showOutput)
						sendObjectTracked(0, 0, 0, 0, 0);
					getLastImageFromBuffer();
					tld->detectorCascade->imgWidth = gray.cols;
					tld->detectorCascade->imgHeight = gray.rows;
					tld->detectorCascade->imgWidthStep = gray.step;

					state = TRACKER_INIT;
				}
				break;
			case TRACKER_INIT:
				if(loadModel && !modelImportFile.empty())
				{
					ROS_INFO("Loading model %s", modelImportFile.c_str());

					tld->readFromFile(modelImportFile.c_str());
					tld->learningEnabled = false;
					state = TRACKING;
				}
				else if(autoFaceDetection || correctBB)
				{
					if(autoFaceDetection)
					{
						target_image = gray;
						target_bb = faceDetection();
					}

					sendObjectTracked(target_bb.x,target_bb.y,target_bb.width,target_bb.height,1.0);

					ROS_INFO("Starting at %d %d %d %d\n", target_bb.x, target_bb.y, target_bb.width, target_bb.height);

					tld->selectObject(target_image, &target_bb);
					tld->learningEnabled = true;
					state = TRACKING;
				}
				else
				{
					ros::Duration(1.0).sleep();
					ROS_INFO("Waiting for a BB");
				}
				break;
			case TRACKING:
				if(newImageReceived())
				{
					ros::Time tic = ros::Time::now();

					getLastImageFromBuffer();
					tld->processImage(img);

					ros::Duration toc = (ros::Time::now() - tic);
					float fps = 1.0/toc.toSec();

					std_msgs::Float32 msg_fps;
					msg_fps.data = fps;
					pub2.publish(msg_fps);

					if(showOutput && tld->currBB != NULL)
					{
						sendObjectTracked(tld->currBB->x,tld->currBB->y,tld->currBB->width,tld->currBB->height,tld->currConf);
					}
				}
				break;
			case STOPPED:
					ros::Duration(1.0).sleep();
					ROS_INFO("Tracker stopped");
				break;
			default:
				break;
		}
	}

	if(exportModelAfterRun)
	{
		tld->writeToFile(modelExportFile.c_str());
	}

	semaphore.unlock();
}