ShapeGraspPlanner::ShapeGraspPlanner(ros::NodeHandle& nh)
{
  /*
   * Gripper model is based on having two fingers, and assumes
   * that the robot is using the moveit_simple_controller_manager
   * gripper interface, with "parallel" parameter set to true.
   */
  nh.param<std::string>("gripper/left_joint", left_joint_, "l_gripper_finger_joint");
  nh.param<std::string>("gripper/right_joint", right_joint_, "r_gripper_finger_joint");
  nh.param("gripper/max_opening", max_opening_, 0.110);
  nh.param("gripper/max_effort", max_effort_, 50.0);
  nh.param("gripper/finger_depth", finger_depth_, 0.02);
  nh.param("gripper/grasp_duration", grasp_duration_, 2.0);
  nh.param("gripper/gripper_tolerance", gripper_tolerance_, 0.02);

  /*
   * Approach is usually aligned with wrist_roll
   */
  nh.param<std::string>("gripper/approach/frame", approach_frame_, "wrist_roll_link");
  nh.param("gripper/approach/min", approach_min_translation_, 0.145);
  nh.param("gripper/approach/desired", approach_desired_translation_, 0.15);

  /*
   * Retreat is usually aligned with wrist_roll
   */
  nh.param<std::string>("gripper/retreat/frame", retreat_frame_, "wrist_roll_link");
  nh.param("gripper/retreat/min", retreat_min_translation_, 0.145);
  nh.param("gripper/retreat/desired", retreat_desired_translation_, 0.15);

  // Distance from tool point to planning frame
  nh.param("gripper/tool_to_planning_frame", tool_offset_, 0.165);
}
    /**
    * Initialize parameters from launch file
    * @param node a ROS node handle
    */
    void Circular::initializeParameters(ros::NodeHandle &node){   
       Descriptor base, range;
       // Loading parameters from a launch file
	   node.param("threshold", this->threshold_, 100); 
       node.param("min_score", this->blob_.min_score, 7);
       node.getParam("landmark_diagonal", this->landmark_diag_); 

        // Base descriptor parameters
       node.getParam("base_shape", base.invariant.shape);
       node.getParam("base_area", base.invariant.area);
       node.getParam("base_hu", base.invariant.hu);
        
       // (Similarity) range descriptor parameters
       node.getParam("range_shape", range.invariant.shape);
       node.getParam("range_area", range.invariant.area);
       node.getParam("range_hu", range.invariant.hu);

       // Rotation of camera in respect to imu
       double roll_deg, pitch_deg, yaw_deg;   
       node.param("rot_x", roll_deg, 0.0);
       node.param("rot_y", pitch_deg, 0.0);
       node.param("rot_z", yaw_deg, 0.0);
       
       this->imuToCamRot_.setRPY(roll_deg*M_PI/180, pitch_deg*M_PI/180, yaw_deg*M_PI/180); 
        
        
       this->blob_.setBase(base);
       this->blob_.setRange(range);

    }
Example #3
0
	void initialize(UAS &uas_,
			ros::NodeHandle &nh,
			diagnostic_updater::Updater &diag_updater)
	{
		double linear_stdev, angular_stdev, orientation_stdev, mag_stdev;

		uas = &uas_;

		nh.param<std::string>("imu/frame_id", frame_id, "fcu");
		nh.param("imu/linear_acceleration_stdev", linear_stdev, 0.0003); // check default by MPU6000 spec
		nh.param("imu/angular_velocity_stdev", angular_stdev, 0.02 * (M_PI / 180.0)); // check default by MPU6000 spec
		nh.param("imu/orientation_stdev", orientation_stdev, 1.0);
		nh.param("imu/magnetic_stdev", mag_stdev, 0.0);

		setup_covariance(linear_acceleration_cov, linear_stdev);
		setup_covariance(angular_velocity_cov, angular_stdev);
		setup_covariance(orientation_cov, orientation_stdev);
		setup_covariance(magnetic_cov, mag_stdev);
		setup_covariance(unk_orientation_cov, 0.0);

		imu_pub = nh.advertise<sensor_msgs::Imu>("imu/data", 10);
		magn_pub = nh.advertise<sensor_msgs::MagneticField>("imu/mag", 10);
		temp_pub = nh.advertise<sensor_msgs::Temperature>("imu/temperature", 10);
		press_pub = nh.advertise<sensor_msgs::FluidPressure>("imu/atm_pressure", 10);
		imu_raw_pub = nh.advertise<sensor_msgs::Imu>("imu/data_raw", 10);
	}
Example #4
0
void readParameters(ros::NodeHandle pnh){
	pnh.param("save_map_launch_package", save_map_launch_package , string("mapping_controller"));
	pnh.param("save_map_launch_file", save_map_launch_file , string("save_map"));
	pnh.param("map_file_path", map_file_path , string("/home/pi"));
	pnh.param("robot_position_topic", robot_position_topic, string("/agent1/amcl_pose"));
	pnh.param("robot_initial_position_topic", robot_initial_position_topic, string("/agent1/initialpose"));
}
bool JacoManipulation::loadParameters(const ros::NodeHandle n)
{
    ROS_DEBUG("Loading parameters");

    n.param("wpi_jaco/arm_name", arm_name_, std::string("jaco"));
    n.param("wpi_jaco/gripper_closed", gripper_closed_, 0.0);
    n.param("wpi_jaco/gripper_open", gripper_open_, 65.0);
    n.param("wpi_jaco/num_fingers", num_fingers_, 3);

    // Update topic prefix
    if (arm_name_ == "jaco2")
      topic_prefix_ = "jaco";
    else
      topic_prefix_ = arm_name_;

    if (kinova_gripper_)
      num_joints_ = num_fingers_ + NUM_JACO_JOINTS;
    else
      num_joints_ = NUM_JACO_JOINTS;

    joint_pos_.resize(num_joints_);

    ROS_INFO("arm_name: %s", arm_name_.c_str());

    ROS_INFO("Parameters loaded.");

    //! @todo MdL [IMPR]: Return is values are all correctly loaded.
    return true;
}
Example #6
0
  CameraImpl(ros::NodeHandle& nh, ros::NodeHandle& nh_private, const openni::DeviceInfo& device_info) :
    rgb_sensor_(new SensorStreamManagerBase()),
    depth_sensor_(new SensorStreamManagerBase()),
    ir_sensor_(new SensorStreamManagerBase()),
    reconfigure_server_(nh_private)
  {
    device_.open(device_info.getUri());

    printDeviceInfo();
    printVideoModes();
    buildResolutionMap();

    device_.setDepthColorSyncEnabled(true);

    std::string rgb_frame_id, depth_frame_id;
    nh_private.param(std::string("rgb_frame_id"), rgb_frame_id, std::string("camera_rgb_optical_frame"));
    nh_private.param(std::string("depth_frame_id"), depth_frame_id, std::string("camera_depth_optical_frame"));

    if(device_.hasSensor(SENSOR_COLOR))
    {
      rgb_sensor_.reset(new SensorStreamManager(nh, device_, SENSOR_COLOR, "rgb", rgb_frame_id, resolutions_[Camera_RGB_640x480_30Hz]));
    }

    if(device_.hasSensor(SENSOR_DEPTH))
    {
      depth_sensor_.reset(new DepthSensorStreamManager(nh, device_, rgb_frame_id, depth_frame_id, resolutions_[Camera_DEPTH_640x480_30Hz]));
    }

    if(device_.hasSensor(SENSOR_IR))
    {
      ir_sensor_.reset(new SensorStreamManager(nh, device_, SENSOR_IR, "ir", depth_frame_id, resolutions_[Camera_IR_640x480_30Hz]));
    }

    reconfigure_server_.setCallback(boost::bind(&CameraImpl::configure, this, _1, _2));
  }
  MyClass()
    : p_nh("~")
  {
    sub = nh.subscribe("/humans/kinect_v2", 1, &MyClass::callback, this);
    recog_sub = nh.subscribe("/humans/recog_info", 1, &MyClass::recogCallback, this);
    dispub =  nh.advertise<std_msgs::Float64>("/displacement", 1); 

    srv = nh.advertiseService("displace", &MyClass::Service, this);    

    pt.x = 5.0;
    pt.y = 0;
    pt.z = 0;
    cv::namedWindow(OPENCV_WINDOW);

    index = 0;
    attention_t_id = 0;
    srv_switch = false;
    p_nh.param("width", width, 1000);
    p_nh.param("height", height, 500);

    o_id = 0;

    prev_time = ros::Time::now();
    now_time = ros::Time::now();
    count_time = 0;
  }
Example #8
0
        HDPCDrive(ros::NodeHandle & nh) : geom(nh) {
            command_pub = nh.advertise<hdpc_com::Commands>("/hdpc_com/commands",1);
            reading_sub = nh.subscribe("/hdpc_com/readings",1,&HDPCDrive::readingsCallback,this);
            state_machine_client = nh.serviceClient<hdpc_com::ChangeStateMachine>("/hdpc_com/changeState");


            control_mode_serv = nh.advertiseService("set_control_mode",&HDPCDrive::set_mode,this);
            direct_command_sub = nh.subscribe("direct",1,&HDPCDrive::directDriveCallback,this);
            ackermann_command_sub = nh.subscribe("ackermann",1,&HDPCDrive::ackermannCallback,this);
            status_pub = nh.advertise<Status>("status",1);

            // TODO: change default value to something that makes sense
            nh.param("max_rotation_speed_rad_per_s",max_rotation_speed_rad_per_s,1.0);
            nh.param("max_linear_speed_m_per_s",max_linear_speed_m_per_s,0.9);
            nh.param("synchronise_steering",synchronise_steering,false);
            nh.param("zero_on_init",zero_on_init,false);

            watchdog = 0;
            desired_elevation = M_PI/2;
            control_mode = HDPCModes::MODE_INIT;
            commands.header.stamp = ros::Time::now();
            for (unsigned int i=0;i<10;i++) {
                commands.isActive[i] = false;
                commands.velocity[i] = 0;
                commands.position[i] = 0.0;
            }
        }
Example #9
0
	CylinderDetection() :
		m_NodeHandle("~")
	{
			m_NodeHandle.param("base_frame", m_strBaseFrame, std::string("/body"));
			m_NodeHandle.param("max_range", m_dMaxRange, 5.0);
			m_NodeHandle.param("n_samples", m_nSamples, 1000);
			m_NodeHandle.param("ransac_tolerance", m_dRansacTolerance, 1.0);
			m_NodeHandle.param("parse_plane_lower_bound_threshhold", THRESHOLD_LOWER_TO_PARSE_PLANE, 10);
			m_NodeHandle.param("parse_plane_upper_bound_threshhold", THRESHOLD_UPPER_TO_PARSE_PLANE, 600);
			m_NodeHandle.param("accept_circle_threshhold", THRESHOLD_TO_ACCEPT_CIRCLE_FOUND, 10);
			m_NodeHandle.param("discretization_precision", m_dDiscretizationPrecision, 0.025);
			m_NodeHandle.param("min_num_valid_sections_to_accept_cylinder", MIN_NUM_VALID_SECTIONS_TO_ACCEPT_CYLINDER, 4);
			m_NodeHandle.param("tolerane_for_plane_concatenation", TOLERANCE_FOR_PLANE_CONCATENATION, 0.01);
			m_NodeHandle.param("maximum_radius", MAXIMUM_RADIUS, 1.0);

			ROS_INFO("Searching for vertical cylinders");
			ROS_INFO("RANSAC: %d iteration with %f tolerance", m_nSamples, m_dRansacTolerance);
			assert(m_nSamples > 0);

			// Make sure TF is ready
			ros::Duration(0.5).sleep();

			m_Subscriber = m_NodeHandle.subscribe("/vrep/depthSensor", 1, &CylinderDetection::pc_callback, this);

			// Initialize the random seed for RANSAC
			srand(time(NULL));

	}
    FollowerFSM() : hold(true),
        currentState(STOPPED),
        nextState(STOPPED),
        nPriv("~")


    {
        //hold = true;
        //currentState = STOPPED;
        //nextState = STOPPED;

        nPriv.param<std::string>("world_frame", world_frame, "world_lol");
        nPriv.param<std::string>("robot_frame", robot_frame, "robot");
        nPriv.param<std::string>("fixed_frame", fixed_frame_id, "odom");
        nPriv.param<double>("minimum_distance", minimum_distance, 2);
        nPriv.param<double>("planner_activation_distance", planner_activation_distance, 1.7);
        nPriv.param("minimum_planner_distance", minimum_planner_distance, 1.7);
        nPriv.param("distance_to_target", distance_to_target, 1.5);
        nPriv.param<double>("kv", kv, 0.03);
        nPriv.param<double>("kalfa", kalfa, 0.08);
        //nPriv.param<double>("rate", frequency, 10);
        rate = new ros::Rate(10.0);

        nPriv.param<std::string>("control_type", cType, "planner");

        if(cType == "planner")
        { ac = new MoveBaseClient("move_base", true);}

        notReceivedNumber = 0;
        sub = n.subscribe("person_position", 1, &FollowerFSM::receiveInformation, this);
        cmdPub = n.advertise<geometry_msgs::Twist>("twist_topic", 1);

        timer = n.createTimer(ros::Duration(0.1), &FollowerFSM::checkInfo, this); //If we didn't receive any position on 2 seconds STOP the robot!
        timer.start();
    }
Example #11
0
void load_params(ros::NodeHandle &nh) {
	string str_calib;
	stringstream ss_calib;

	nh.param("D", str_calib, string(""));
	restoreVec(str_calib, cam_info.D);

	nh.param("K", str_calib, string(""));
	 (str_calib, cam_info.K);

	nh.param("R", str_calib, string(""));
	restoreVec(str_calib, cam_info.R);

	nh.param("P", str_calib, string(""));
	restoreVec(str_calib, cam_info.P);
	
	std::string frame_id;
	nh.param<std::string>("frame_id",frame_id,"/camera");// /viz/uav_cam_front/camera
	cam_info.header.frame_id = frame_id;

	cout
			<< "Current cam_info:"
			<< endl;
	cout << "    <param name=\"D\" type=\"string\" value=\"" << arrayToString(
			cam_info.D) << "\"/>" << endl;
	cout << "    <param name=\"K\" type=\"string\" value=\"" << arrayToString(
			cam_info.K) << "\"/>" << endl;
	cout << "    <param name=\"R\" type=\"string\" value=\"" << arrayToString(
			cam_info.R) << "\"/>" << endl;
	cout << "    <param name=\"P\" type=\"string\" value=\"" << arrayToString(
			cam_info.P) << "\"/>" << endl;
}
int NodeClass::init() 
{
	if (n.hasParam("ComPort"))
	{
		n.getParam("ComPort", sComPort);
		ROS_INFO("Loaded ComPort parameter from parameter server: %s",sComPort.c_str());
	}
	else
	{
		sComPort ="/dev/ttyUSB0";
		ROS_WARN("ComPort Parameter not available, using default Port: %s",sComPort.c_str());
	}

	n.param("message_timeout", relayboard_timeout_, 2.0);

	n.param("protocol_version", protocol_version_, 1);
    
	m_SerRelayBoard = new SerRelayBoard(sComPort, protocol_version_);
	ROS_INFO("Opened Relayboard at ComPort = %s", sComPort.c_str());

	m_SerRelayBoard->init();

	// Init member variable for EM State
	EM_stop_status_ = ST_EM_ACTIVE;
	duration_for_EM_free_ = ros::Duration(1);

	return 0;
}
WebVideoServer::WebVideoServer(ros::NodeHandle &nh, ros::NodeHandle &private_nh) :
    nh_(nh), image_transport_(nh),
    handler_group_(http_server::HttpReply::stock_reply(http_server::HttpReply::not_found))
{
  cleanup_timer_ = nh.createTimer(ros::Duration(0.5), boost::bind(&WebVideoServer::cleanup_inactive_streams, this));

  int port;
  private_nh.param("port", port, 8080);

  int server_threads;
  private_nh.param("server_threads", server_threads, 1);

  private_nh.param("ros_threads", ros_threads_, 2);

  stream_types_["mjpeg"] = boost::shared_ptr<ImageStreamerType>(new MjpegStreamerType());
  stream_types_["vp8"] = boost::shared_ptr<ImageStreamerType>(new LibavStreamerType("webm", "libvpx", "video/webm"));

  handler_group_.addHandlerForPath("/", boost::bind(&WebVideoServer::handle_list_streams, this, _1, _2));
  handler_group_.addHandlerForPath("/stream", boost::bind(&WebVideoServer::handle_stream, this, _1, _2));
  handler_group_.addHandlerForPath("/stream_viewer", boost::bind(&WebVideoServer::handle_stream_viewer, this, _1, _2));
  handler_group_.addHandlerForPath("/snapshot", boost::bind(&WebVideoServer::handle_snapshot, this, _1, _2));

  server_.reset(new http_server::HttpServer("0.0.0.0", boost::lexical_cast<std::string>(port),
      boost::bind(ros_connection_logger, handler_group_, _1, _2), server_threads));
}
Example #14
0
//! Initialize plugin - called in server constructor
void srs_env_model::CCMapPlugin::init(ros::NodeHandle & node_handle)
{
	m_collisionMapVersion = 0;
	m_dataBuffer->boxes.clear();
	m_data->boxes.clear();

	// Read parameters

	// Get collision map radius
	node_handle.param("collision_map_radius", m_collisionMapLimitRadius, COLLISION_MAP_RADIUS_LIMIT );

	// Get collision map topic name
	node_handle.param("collision_map_publisher", m_cmapPublisherName, COLLISION_MAP_PUBLISHER_NAME );

	// Get FID to which will be points transformed when publishing collision map
	node_handle.param("collision_map_frame_id", m_cmapFrameId, COLLISION_MAP_FRAME_ID ); //

	// Create and publish service - get collision map
	m_serviceGetCollisionMap = node_handle.advertiseService( GetCollisionMap_SRV, 	&CCMapPlugin::getCollisionMapSrvCallback, this);

	// Create and publish service - is new collision map
	m_serviceIsNewCMap = node_handle.advertiseService( IsNewCMap_SRV, &CCMapPlugin::isNewCmapSrvCallback, this );

	// Create and publish service - lock map
	m_serviceLockCMap = node_handle.advertiseService( LockCMap_SRV, &CCMapPlugin::lockCmapSrvCallback, this );

	// Create and publish service - remove cube from map
	m_serviceRemoveCube = node_handle.advertiseService( RemoveCubeCMP_SRV, &CCMapPlugin::removeBoxCallback, this );

	// Create and publish service - add cube to map
	m_serviceAddCube = node_handle.advertiseService( AddCubeCMP_SRV, &CCMapPlugin::addBoxCallback, this );

	// Connect publishing services
	pause( false, node_handle );
}
void FreeFloatingPids::InitPID(control_toolbox::Pid &_pid, const ros::NodeHandle&_node, const bool &_use_dynamic_reconfig)
{
    if(_use_dynamic_reconfig)
    {
        // classical PID init
        _pid.init(_node);
    }
    else
    {
        control_toolbox::Pid::Gains gains;

        // Load PID gains from parameter server
        if (!_node.getParam("p", gains.p_gain_))
        {
          ROS_ERROR("No p gain specified for pid.  Namespace: %s", _node.getNamespace().c_str());
          return;
        }
        // Only the P gain is required, the I and D gains are optional and default to 0:
        _node.param("i", gains.i_gain_, 0.0);
        _node.param("d", gains.d_gain_, 0.0);

        // Load integral clamp from param server or default to 0
        double i_clamp;
        _node.param("i_clamp", i_clamp, 0.0);
        gains.i_max_ = std::abs(i_clamp);
        gains.i_min_ = -std::abs(i_clamp);
        _pid.setGains(gains);
    }
}
void PlaDyPosNode_v3::configure(ros::NodeHandle& nh, ros::NodeHandle& ph)
{
	//Initialize subscribers and publishers
	tau = nh.subscribe("tauIn", 1, &PlaDyPosNode_v3::onTau,this);
	batteryVoltage = nh.subscribe("battery_voltage", 1, &PlaDyPosNode_v3::onBatteryVoltage,this);
	tauAch = nh.advertise<auv_msgs::BodyForceReq>("tauAch",1);
	revs = nh.advertise<std_msgs::Int16MultiArray>("pwm_out",1);
	
	//Initialize max/min tau
	double maxThrust(1),minThrust(-1);
	//ph.param("maxThrust",maxThrust,maxThrust);
	//ph.param("minThrust",minThrust,minThrust);
	ph.param("max_cap",maxCap,maxCap);
	ph.param("useWeighted",useWeighted, useWeighted);

	//Initialize the allocation matrix
	float cp(cos(M_PI/4)),sp(sin(M_PI/4));
	B<<cp,cp,-cp,-cp,
	   -sp,sp,-sp,sp,
	    -1,1,1,-1;

	BinvIdeal = B.transpose()*(B*B.transpose()).inverse();

	//Scaling allocation only for XYN
	allocator.configure(B,maxThrust,minThrust);
}
  virtual void onInit()
  {
    nh_ = getNodeHandle();
    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
    local_nh_ = ros::NodeHandle("~");

    local_nh_.param("debug_view", debug_view_, false);
    subscriber_count_ = 0;
    prev_stamp_ = ros::Time(0, 0);

    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&FaceDetectionNodelet::img_connectCb, this, _1);
    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&FaceDetectionNodelet::img_disconnectCb, this, _1);
    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&FaceDetectionNodelet::msg_connectCb, this, _1);
    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&FaceDetectionNodelet::msg_disconnectCb, this, _1);
    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
    msg_pub_ = local_nh_.advertise<opencv_apps::FaceArrayStamped>("faces", 1, msg_connect_cb, msg_disconnect_cb);
        
    if( debug_view_ ) {
      subscriber_count_++;
    }
    std::string face_cascade_name, eyes_cascade_name;
    local_nh_.param("face_cascade_name", face_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml"));
    local_nh_.param("eyes_cascade_name", eyes_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_eye_tree_eyeglasses.xml"));
        
    if( !face_cascade_.load( face_cascade_name ) ){ NODELET_ERROR("--Error loading %s", face_cascade_name.c_str()); };
    if( !eyes_cascade_.load( eyes_cascade_name ) ){ NODELET_ERROR("--Error loading %s", eyes_cascade_name.c_str()); };

    dynamic_reconfigure::Server<face_detection::FaceDetectionConfig>::CallbackType f =
      boost::bind(&FaceDetectionNodelet::reconfigureCallback, this, _1, _2);
    srv.setCallback(f);
  }
bool ExcavaROBArmKinematics::init() {
    // Get URDF XML
    std::string urdf_xml, full_urdf_xml;
    nh_.param("urdf_xml", urdf_xml, std::string("robot_description"));
    nh_.searchParam(urdf_xml, full_urdf_xml);
    ROS_DEBUG("ExcavaROB Kinematics: Reading xml file from parameter server");
    std::string result;
    if (!nh_.getParam(full_urdf_xml, result)) {
        ROS_FATAL("ExcavaROB Kinematics: Could not load the xml from parameter server: %s", urdf_xml.c_str());
	return false;
    }

    // Get Root, Wrist and Tip From Parameter Service
    if (!nh_private_.getParam("root_name", root_name_)) {
        ROS_FATAL("ExcavaROB Kinematics: No root_name found on parameter server");
        return false;
    }
    if (!nh_private_.getParam("tip_name", tip_name_)) {
        ROS_FATAL("ExcavaROB Kinematics: No tip_name found on parameter server");
        return false;
    }
    if (!nh_private_.getParam("max_iterations", max_iterations_)) {
        ROS_FATAL("ExcavaROB Kinematics: No max_iterations found on parameter server");
        return false;
    }
    if (!nh_private_.getParam("eps", eps_)) {
        ROS_FATAL("ExcavaROB Kinematics: No eps found on parameter server");
        return false;
    }
    if (!nh_private_.getParam("ik_gain", ik_gain_)) {
        ROS_FATAL("ExcavaROB Kinematics: No ik_gain found on parameter server");
        return false;
    }

    // Load and Read Models
    if (!loadModel(result)) {
        ROS_FATAL("Could not load models!");
        return false;
    }

    // Get Solver Parameters
    int maxIterations;
    double epsilon;
    nh_private_.param("maxIterations", maxIterations, 1000);
    nh_private_.param("epsilon", epsilon, 1e-2);

    // Build Solvers
    fk_solver_ = new KDL::ChainFkSolverPos_recursive(arm_chain_);
    jnt_to_jac_solver_ = new KDL::ChainJntToJacSolver(arm_chain_);
    jnt_to_pose_solver_ = new KDL::ChainFkSolverPos_recursive(arm_chain_);

    ROS_INFO("ExcavaROB Kinematics: Advertising services");
    ik_service_ = nh_private_.advertiseService(IK_SERVICE, &ExcavaROBArmKinematics::getPositionIK, this);
    fk_service_ = nh_private_.advertiseService(FK_SERVICE, &ExcavaROBArmKinematics::getPositionFK, this);

    kin_solver_info_service_ = nh_private_.advertiseService(KIN_INFO_SERVICE, &ExcavaROBArmKinematics::getKinSolverInfo, this);

    return true;
}
void srs_env_model::CCollisionObjectPlugin::init(ros::NodeHandle & node_handle)
{
	node_handle.param("collision_object_publisher", m_coPublisherName, COLLISION_OBJECT_PUBLISHER_NAME );
	node_handle.param("collision_object_frame_id", m_coFrameId, COLLISION_OBJECT_FRAME_ID );

	// Create publisher
	m_coPublisher = node_handle.advertise<arm_navigation_msgs::CollisionObject> (m_coPublisherName, 100, m_latchedTopics);
}
bool Kinematics::init() {
    // Get URDF XML
    std::string urdf_xml, full_urdf_xml;
    nh.param("urdf_xml",urdf_xml,std::string("robot_description"));
    nh.searchParam(urdf_xml,full_urdf_xml);
    ROS_DEBUG("Reading xml file from parameter server");
    std::string result;
    if (!nh.getParam(full_urdf_xml, result)) {
        ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
        return false;
    }
 
    std::string kinematics_service_name;
    if (!nh_private.getParam("kinematics_service_name", kinematics_service_name)) {
        ROS_FATAL("GenericIK: kinematics service name not found on parameter server");
        return false;
    }
    // Get Root and Tip From Parameter Service
    if (!nh.getParam(kinematics_service_name+"/manipulator/root_name", root_name)) {
        ROS_FATAL("GenericIK: No root name found on parameter server");
        return false;
    }
    if (!nh.getParam(kinematics_service_name+"/manipulator/tip_name", tip_name)) {
        ROS_FATAL("GenericIK: No tip name found on parameter server");
        return false;
    }

    // Load and Read Models
    if (!loadModel(result)) {
        ROS_FATAL("Could not load models!");
        return false;
    }

    // Get Solver Parameters
    int maxIterations;
    double epsilon;

    nh_private.param("maxIterations", maxIterations, 1000);
    nh_private.param("epsilon", epsilon, 1e-2); // .01

    // Build Solvers
    fk_solver = new KDL::ChainFkSolverPos_recursive(chain);
    ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain);

    ik_solver_pos = new KDL::ChainIkSolverPos_NR_JL(chain, joint_min, joint_max, *fk_solver, *ik_solver_vel, maxIterations, epsilon);

    ROS_INFO("Advertising services");
    fk_service = nh_private.advertiseService(FK_SERVICE,&Kinematics::getPositionFK,this);

    // ik_service = nh_private.advertiseService(IK_SERVICE,&Kinematics::getPositionIK,this);
    ik_service = nh_private.advertiseService(IK_SERVICE,&Kinematics::searchPositionIK,this);

    ik_solver_info_service = nh_private.advertiseService(IK_INFO_SERVICE,&Kinematics::getIKSolverInfo,this);
    fk_solver_info_service = nh_private.advertiseService(FK_INFO_SERVICE,&Kinematics::getFKSolverInfo,this);

    return true;
}
void
fillCameraInfoMessage (ros::NodeHandle nh, std::string base_name, sensor_msgs::CameraInfo& camera_info_msg )
{
  // Read calibration data and fill camera_info message:
  int width, height;
  nh.param(base_name + "/image_width", width, 640);
  nh.param(base_name + "/image_height", height, 480);
  camera_info_msg.width = width;
  camera_info_msg.height = height;

  std::string distortion_model;
  if (not nh.getParam (base_name + "/distortion_model", distortion_model))
  {
    ROS_ERROR("Stereo calibration parameters not found!");
  }
  camera_info_msg.distortion_model = distortion_model;

  std::vector<float> camera_matrix;
  if (not nh.getParam (base_name + "/camera_matrix/data", camera_matrix))
  {
    ROS_ERROR("Stereo calibration parameters not found!");
  }
  for (unsigned int i = 0; i < camera_matrix.size(); i++)
  {
    camera_info_msg.K[i] = camera_matrix[i];
  }

  std::vector<float> rectification_matrix;
  if (not nh.getParam (base_name + "/rectification_matrix/data", rectification_matrix))
  {
    ROS_ERROR("Stereo calibration parameters not found!");
  }
  for (unsigned int i = 0; i < rectification_matrix.size(); i++)
  {
    camera_info_msg.R[i] = rectification_matrix[i];
  }

  std::vector<float> projection_matrix;
  if (not nh.getParam (base_name + "/projection_matrix/data", projection_matrix))
  {
    ROS_ERROR("Stereo calibration parameters not found!");
  }
  for (unsigned int i = 0; i < projection_matrix.size(); i++)
  {
    camera_info_msg.P[i] = projection_matrix[i];
  }

  std::vector<float> distortion_coefficients;
  if (not nh.getParam (base_name + "/distortion_coefficients/data", distortion_coefficients))
  {
    ROS_ERROR("Stereo calibration parameters not found!");
  }
  for (unsigned int i = 0; i < distortion_coefficients.size(); i++)
  {
    camera_info_msg.D.push_back(distortion_coefficients[i]);
  }
}
CJointController::CJointController(std::string name, CQboduinoDriver *device_p, ros::NodeHandle& nh, std::map<std::string,CServo *>& servos) : CController(name,device_p,nh)
{
    joints_dirty_=true;
    servos_p_=&servos;
    std::string topic;
    nh.param("controllers/"+name+"/topic", topic, std::string("cmd_joints"));
    nh.param("controllers/"+name+"/rate", rate_, 15.0);
    joint_sub_ = nh.subscribe<sensor_msgs::JointState>(topic, 10, &CJointController::jointCallback, this);
    timer_=nh.createTimer(ros::Duration(1/rate_),&CJointController::timerCallback,this);
}
Example #23
0
void srs_env_model::CMap2DPlugin::init(ros::NodeHandle & node_handle)
{
	node_handle.param("collision_object_publisher", m_map2DPublisherName, MAP2D_PUBLISHER_NAME );
	node_handle.param("collision_object_frame_id", m_map2DFrameId, MAP2D_FRAME_ID );
	node_handle.param("min_x_size", m_minSizeX, m_minSizeX);
	node_handle.param("min_y_size", m_minSizeY, m_minSizeY);

	// Create publisher
	m_map2DPublisher = node_handle.advertise<nav_msgs::OccupancyGrid> (m_map2DPublisherName, 100, m_latchedTopics);
}
	void init() {

		nh_.param("scale_angular", a_scale_, 1.0);
		nh_.param("scale_linear", l_scale_, 0.23);
		nh_.param("walk_scale", walk_scale_, 0.6);

		vel_pub_ = nh_.advertise<geometry_msgs::Twist> ("cmd_vel", 1);

		ros::NodeHandle n_private("~");
	}
CBatteryController::CBatteryController(std::string name, CQboduinoDriver *device_p, ros::NodeHandle& nh) : CController(name,device_p,nh)
{
    level_=0;
    stat_=0;
    std::string topic;
    nh.param("controllers/"+name+"/topic", topic, std::string("battery_state"));
    nh.param("controllers/"+name+"/rate", rate_, 15.0);
    battery_pub_ = nh.advertise<qbo_arduqbo::BatteryLevel>(topic, 1);
    timer_=nh.createTimer(ros::Duration(1/rate_),&CBatteryController::timerCallback,this);
}
Example #26
0
//! Initialize plugin - called in server constructor
void srs_env_model::CPointCloudPlugin::init(ros::NodeHandle & node_handle)
{
//	PERROR( "Initializing PointCloudPlugin" );

	// Frame skipping
	int fs( m_use_every_nth );
	node_handle.param( "pointcloud_frame_skip", fs, 1 );
//	m_use_every_nth = (fs >= 1) ? fs : 1;

	// Point cloud publishing topic name
	node_handle.param("pointcloud_centers_publisher", m_pcPublisherName, POINTCLOUD_CENTERS_PUBLISHER_NAME );

	// Point cloud subscribing topic name - try to get it from parameter server if not given
	if( m_pcSubscriberName.length() == 0 )
		node_handle.param("pointcloud_subscriber", m_pcSubscriberName, SUBSCRIBER_POINT_CLOUD_NAME);
	else
		m_bSubscribe = true;

	// Get FID to which will be points transformed when publishing collision map
	node_handle.param("pointcloud_frame_id", m_pcFrameId, m_pcFrameId ); //

	// Point cloud limits
	node_handle.param("pointcloud_min_z", m_pointcloudMinZ, m_pointcloudMinZ);
	node_handle.param("pointcloud_max_z", m_pointcloudMaxZ, m_pointcloudMaxZ);

	// Create publisher
	m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 1, m_latchedTopics);


	// If should subscribe, create message filter and connect to the topic
	if( m_bSubscribe )
	{
//		PERROR("Subscribing to: " << m_pcSubscriberName );
		// Create subscriber
		m_pcSubscriber  = new message_filters::Subscriber<tIncommingPointCloud>(node_handle, m_pcSubscriberName, 1);

		if (!m_pcSubscriber)
		{
			ROS_ERROR("Not subscribed...");
			PERROR( "Not subscirbed to point clouds subscriber...");
		}

		// Create message filter
		m_tfPointCloudSub = new tf::MessageFilter<tIncommingPointCloud>( *m_pcSubscriber, m_tfListener, m_pcFrameId, 1);
		m_tfPointCloudSub->registerCallback(boost::bind( &CPointCloudPlugin::insertCloudCallback, this, _1));

		//std::cerr << "SUBSCRIBER NAME: " << m_pcSubscriberName << ", FRAMEID: " << m_pcFrameId << std::endl;
	}

	// Clear old pointcloud data
	m_data->clear();

//	PERROR( "PointCloudPlugin initialized..." );

}
    MovePlatformAction() :
        as_(n_, CARLOS_MOVE_ACTION, false), //movePlatform action SERVER
        ac_planner_("/plan_action", true), // Planner action CLIENT
        ac_control_("/control_action", true) // Controller action CLIENT
    {

        n_.param("/move_platform_server/debug", debug_, false);

        std::string name = ROSCONSOLE_DEFAULT_NAME; //ros.carlos_motion_action_server
        name = name  + ".debug";
        logger_name_ = "debug";
        //logger is ros.carlos_motion_action_server.debug

        if (debug_)
        {
            // if we use ROSCONSOLE_DEFAULT_NAME we'll get a ton of debug messages from actionlib which is annoying!!!
            // so for debug we'll use a named logger
            if(ros::console::set_logger_level(name, ros::console::levels::Debug)) //name
                ros::console::notifyLoggerLevelsChanged();
        }
        else // if not DEBUG we want INFO
        {
            if(ros::console::set_logger_level(name, ros::console::levels::Info)) //name
                ros::console::notifyLoggerLevelsChanged();
        }

        ROS_DEBUG_NAMED(logger_name_, "Starting Move Platform Server");

        as_.registerGoalCallback(boost::bind(&MovePlatformAction::moveGoalCB, this));
        as_.registerPreemptCallback(boost::bind(&MovePlatformAction::movePreemptCB, this));

        //start the move server
        as_.start();
        ROS_DEBUG_NAMED(logger_name_, "Move Platform Action Server Started");

        // now wait for the other servers (planner + controller) to start
        ROS_WARN_NAMED(logger_name_, "Waiting for planner server to start");
        ac_planner_.waitForServer();
        ROS_INFO_STREAM_NAMED(logger_name_, "Planner server started: " <<  ac_planner_.isServerConnected());

        ROS_WARN_NAMED(logger_name_, "Waiting for controller server to start");
        ac_control_.waitForServer();
        ROS_INFO_STREAM_NAMED(logger_name_, "Controller server started: " <<  ac_control_.isServerConnected());

        n_.param("/carlos/fsm_frequency", frequency_, DEFAULT_STATE_FREQ);
        state_pub_timer_ = n_.createTimer(frequency_, &MovePlatformAction::state_pub_timerCB, this);
        state_pub_ = n_.advertise<mission_ctrl_msgs::hardware_state>(CARLOS_BASE_STATE_MSG,1);

        planning_ = false;
        controlling_ = false;
        //set_terminal_state_;
        ctrl_state_sub = n_.subscribe<std_msgs::String>("/oea_controller/controller_state", 5, &MovePlatformAction::control_stateCB, this);
        planner_state_sub = n_.subscribe<std_msgs::UInt8>("/oea_planner/state", 5, &MovePlatformAction::planner_stateCB, this);

    }
// constructor
TOea_Planner::TOea_Planner(ros::NodeHandle &n, ros::NodeHandle &private_n, std::string logger_name) : Astar_(logger_name)//: plan_("/move_platform", n)
{
    logger_name_ = logger_name;
    ros::start();

    //load parameters:
    private_n.param<std::string>("global_frame_id", global_frame_id, "map");
    private_n.param<std::string>("base_frame_id", base_frame_id, "base_footprint");
    private_n.param("inflate_map_borders", Astar_.inflate_map_borders_, false);
    private_n.param("robot_x_size", Astar_.robot_x_size_, 1.2);//1.2
    private_n.param("robot_y_size", Astar_.robot_y_size_, 0.8);//0.8
    private_n.param("allow_unknown", Astar_.allow_unknown_, true);//0.8
    private_n.param("stop_at_exact_target", Astar_.stop_at_exact_target_, false);
    private_n.param("publish_entire_pcd", Astar_.publish_entire_pcd_, false);//0.8
    private_n.param("use_frontal_laser", Astar_.use_frontal_laser, true);
    private_n.param("use_back_laser", Astar_.use_back_laser, true);
    private_n.param("use_localization", use_localization, true);
    private_n.param("mark_end_cubes", mark_end_cubes, true);


    //TODO: DEBUG THIS
    //print parameters
   /* if (true)
    {
        std::cout << BOLDMAGENTA << " Parameters: " << RESET << std::endl;
        std::cout << BOLDWHITE   << "  * global_frame_id: " << RESET << global_frame_id << std::endl;
        std::cout << BOLDWHITE   << "  * base_frame_id: " << RESET << base_frame_id <<  std::endl;
        std::cout << BOLDWHITE   << "  * robot_x_size: " << RESET << Astar_.robot_x_size_ <<  std::endl;
        std::cout << BOLDWHITE   << "  * robot_y_size: " << RESET << Astar_.robot_y_size_ <<  std::endl;
        std::cout << BOLDWHITE   << "  * inflate_map_borders: " << RESET << Astar_.inflate_map_borders_ <<  std::endl;
        std::cout << BOLDWHITE   << "  * allow_unknown_: " << RESET << Astar_.allow_unknown_ <<  std::endl;
        std::cout << BOLDWHITE   << "  * stop_at_exact_target: " << RESET << Astar_.stop_at_exact_target_ << std::endl;
        std::cout << BOLDWHITE   << "  * publish_entire_pcd: " << RESET << Astar_.publish_entire_pcd_ << std::endl;
        std::cout << BOLDWHITE   << "  * use_frontal_laser: " << RESET << Astar_.use_frontal_laser << std::endl;
        std::cout << BOLDWHITE   << "  * use_back_laser: " << RESET << Astar_.use_back_laser <<  std::endl;
        std::cout << BOLDWHITE << " >>>>>>>>>>>>>>>>>< level_closest: " << Astar_.level_closest << RESET << std::endl;
        std::cout << BOLDWHITE << " >>>>>>>>>>>>>>>>>< level_middle: " << Astar_.level_middle << RESET << std::endl;
        std::cout << BOLDWHITE << " >>>>>>>>>>>>>>>>>< level_farthest: " << Astar_.level_farthest << RESET << std::endl;
        std::cout << BOLDWHITE <<" ****************************************" << RESET << std::endl;
    }*/


    // subscribers:
    goal_sub_ = n.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, &TOea_Planner::goalCB, this);
    start_pose_sub_ = n.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/start_pose", 1, &TOea_Planner::start_poseCB, this);
    map_sub_ = n.subscribe<nav_msgs::OccupancyGrid>("/map", 100, &TOea_Planner::mapCB, this);

    // publishers:
    pcd_pub_ =  private_n.advertise<sensor_msgs::PointCloud2>("inflation_cloud", 1, true); // inflation point cloud (latched)
    oea_path_pub_ = private_n.advertise<oea_msgs::Oea_path>("oea_plan",1); //publishing this topic causes the controller to start
    visual_path_pub_ = n.advertise<nav_msgs::Path>("/plan",1, true); //latched topic, same as for the actions -> this way we can have only one topic for visualization
    arrows_pub_ = private_n.advertise<visualization_msgs::MarkerArray>("arrows_marker_array", 1, true);
    state_pub_ = private_n.advertise<std_msgs::UInt8>("state", 1);
    cells_pub_ = private_n.advertise<visualization_msgs::MarkerArray>("cells_marker_array", 1, true);
    // services:
    ss_ = private_n.advertiseService("IsPoseValid", &TOea_Planner::IsPoseValid, this);

    map_received_ = false;

}
Example #29
0
SegmentPlane::SegmentPlane(ros::NodeHandle nh, ros::NodeHandle n)
  : nh_(nh), rate_(n.param("loop_rate", 10)),
    frame_id_(n.param<std::string>("inliers_pc_frame_id", "/inliers_pc_frame"))
{
  leaf_size_x_ = n.param("leaf_size_x", 0.3);
  leaf_size_y_ = n.param("leaf_size_y", 0.3);
  leaf_size_z_ = n.param("leaf_size_z", 0.3);
  
  source_pc_sub_ = nh_.subscribe(n.param<std::string>("source_pc_topic_name", "/source_pointcloud"), 1, &SegmentPlane::detectPlaneCallback, this);
  inliers_pc_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(n.param<std::string>("inliers_pc_topic_name", "/inliers_pointcloud"), 1);
}
Example #30
0
KinectV2Downsampler::KinectV2Downsampler(ros::NodeHandle nh, ros::NodeHandle n)
  : nh_(nh), rate_(n.param("loop_rate", 10)),
    frame_id_(n.param<std::string>("resized_pc_frame_id", "/resized_pc_frame"))
{
  leaf_size_x_ = n.param("leaf_size_x", 0.3);
  leaf_size_y_ = n.param("leaf_size_y", 0.3);
  leaf_size_z_ = n.param("leaf_size_z", 0.3);

  source_pc_sub_ = nh_.subscribe(n.param<std::string>("source_pc_topic_name", "/source_pointcloud"), 1, &KinectV2Downsampler::resizeCallback, this);
  resized_pc_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(n.param<std::string>("resized_pc_topic_name", "/resized_pointcloud"), 1);
}