CoaxSimpleControl::CoaxSimpleControl(ros::NodeHandle &node)
    :reach_nav_state(node.serviceClient<coax_msgs::CoaxReachNavState>("reach_nav_state"))
    ,configure_comm(node.serviceClient<coax_msgs::CoaxConfigureComm>("configure_comm"))
    ,configure_control(node.serviceClient<coax_msgs::CoaxConfigureControl>("configure_control"))
    ,set_timeout(node.serviceClient<coax_msgs::CoaxSetTimeout>("set_timeout"))

    ,coax_state_sub(node.subscribe("state",1, &CoaxSimpleControl::coaxStateCallback, this))
    ,coax_tf_sub(node.subscribe("tf",1, &CoaxSimpleControl::coaxTfCallback, this))

    ,raw_control_pub(node.advertise<coax_msgs::CoaxRawControl>("rawcontrol",1))
    ,simple_control_pub(node.advertise<coax_msgs::CoaxControl>("simplecontrol",1))

    ,LOW_POWER_DETECTED(false)
    ,CONTROL_MODE(CONTROL_LANDED)
    ,FIRST_START(false)
    ,FIRST_LANDING(false)
    ,FIRST_HOVER(false)
    ,INIT_DESIRE(false)
    ,coax_nav_mode(0)
    ,coax_control_mode(0)
    ,coax_state_age(0)
    ,raw_control_age(0)
    ,init_count(0)
    ,mil_count(0)
    ,rotor_ready_count(-1)
    ,battery_voltage(12.22)
    ,imu_y(0.0),imu_r(0.0),imu_p(0.0)
    ,range_al(0.0)
    ,rc_th(0.0),rc_y(0.0),rc_r(0.0),rc_p(0.0)
    ,rc_trim_th(0.0),rc_trim_y(0.104),rc_trim_r(0.054),rc_trim_p(0.036)
    ,img_th(0.0),img_y(0.0),img_r(0.0),img_p(0.0)
    ,gyro_ch1(0.0),gyro_ch2(0.0),gyro_ch3(0.0)
    ,accel_x(0.0),accel_y(0.0),accel_z(0.0)
    ,mag_1(0.0), mag_2(0.0), mag_3(0.0)
    ,motor_up(0),motor_lo(0)
    ,servo_roll(0),servo_pitch(0)
    ,roll_trim(0),pitch_trim(0)
    ,motor1_des(0.0),motor2_des(0.0),servo1_des(0.0),servo2_des(0.0)
    ,yaw_des(0.0),yaw_rate_des(0.0)
    ,roll_des(0.0),roll_rate_des(0.0)
    ,pitch_des(0.0),pitch_rate_des(0.0)
    ,altitude_des(1.5), coax_global_x(0.0), coax_global_y(0.0), coax_global_z(0.0)
    ,qnd0(0.0), qnd1(0.0), qnd2(0.0), qnd3(0.0)
    ,Gx_des(0.0), Gy_des(0.0)
    ,PI(3.14159265), r2d(57.2958), d2r(0.0175)
    ,ang_err_lim(0.14)
    ,x_vel_hist_1(0.0), x_vel_hist_2(0.0), x_vel_hist_3(0.0) , x_vel_hist_4(0.0)
    ,y_vel_hist_1(0.0), y_vel_hist_2(0.0), y_vel_hist_3(0.0) , y_vel_hist_4(0.0)
    ,x_gyro_1(0.0), x_gyro_2(0.0)
    ,y_gyro_1(0.0), y_gyro_2(0.0)
    ,Gz_old1(0.0), Gz_old2(0.0), Gz_old3(0.0)
    ,way_changed(0)
    ,way_old{0.0, 0.0, 0.0}

{
    set_nav_mode.push_back(node.advertiseService("set_nav_mode", &CoaxSimpleControl::setNavMode, this));
    set_control_mode.push_back(node.advertiseService("set_control_mode", &CoaxSimpleControl::setControlMode, this));
    set_waypoint.push_back(node.advertiseService("set_waypoint", &CoaxSimpleControl::setWaypoint, this));
    loadParams(node);
}
  PeoplePositionServer()
  {
    rein_sub_ 
      = n.subscribe("/humans/RecogInfo", 1, 
		    &PeoplePositionServer::callback, this);
    face_now_pub_
      = n.advertise<humans_msgs::PersonPoseImgArray
		    >("/now_human", 10);

    track_srv_ 
      = n.advertiseService("track_srv", 
			  &PeoplePositionServer::resTrackingId, this);
    okao_srv_ 
      = n.advertiseService("okao_srv", 
			   &PeoplePositionServer::resOkaoId, this);

     name_srv_ 
       = n.advertiseService("name_srv", 
			    &PeoplePositionServer::resName, this);
     
    okaoStack_
      = n.serviceClient<okao_client::OkaoStack>("stack_send");

    //array_srv_ 
    // = n.advertiseService("array_srv", 
    //			   &PeoplePositionServer::resHumans, this);

    
  }
Exemplo n.º 3
0
OnboardControl::OnboardControl(ros::NodeHandle &node)
:

 reach_nav_state(node.serviceClient<coax_msgs::CoaxReachNavState>("reach_nav_state"))
,configure_comm(node.serviceClient<coax_msgs::CoaxConfigureComm>("configure_comm"))
,configure_control(node.serviceClient<coax_msgs::CoaxConfigureControl>("configure_control"))
,set_timeout(node.serviceClient<coax_msgs::CoaxSetTimeout>("set_timeout"))
,onboard_stage_pub(node.advertise<coax_msgs::stageState>("onboard_stage",1))
,LOW_POWER_DETECTED(false)
,CONTROL_MODE(CONTROL_LANDED)
,FIRST_START(false)
,FIRST_STATE(true)
,FIRST_LANDING(false)
,FIRST_HOVER(false)
,INIT_DESIRE(false)
,INIT_IMU(false)
,coax_nav_mode(0)
,coax_control_mode(0)
,coax_state_age(0)
,raw_control_age(0)
,init_count(0)
,init_imu_count(0)
,last_state_time(0.0)
{  
	set_nav_mode = node.advertiseService("set_nav_mode", &OnboardControl::setNavMode, this);
	set_control_mode = node.advertiseService("set_control_mode", &OnboardControl::setControlMode, this);
}
Exemplo n.º 4
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 );
}
Exemplo n.º 5
0
    SnapshotMap( ros::NodeHandle& nh ) : nh_( nh ),object_available(false) {

		imageAllocator_ = boost::shared_ptr< MultiResolutionSurfelMap::ImagePreAllocator >( new MultiResolutionSurfelMap::ImagePreAllocator() );
		treeNodeAllocator_ = boost::shared_ptr< spatialaggregate::OcTreeNodeDynamicAllocator< float, MultiResolutionSurfelMap::NodeValue > >( new spatialaggregate::OcTreeNodeDynamicAllocator< float, MultiResolutionSurfelMap::NodeValue >( 10000 ) );

		snapshot_service_ = nh.advertiseService("snapshot", &SnapshotMap::snapshotRequest, this);
		object_pose_service_ = nh.advertiseService("object_pose", &SnapshotMap::poseRequest, this);
		pub_cloud = pcl_ros::Publisher<pcl::PointXYZRGB>(nh, "output_cloud", 1);

		pub_status_ = nh.advertise< std_msgs::Int32 >( "status", 1 );

		tf_listener_ = boost::shared_ptr< tf::TransformListener >( new tf::TransformListener() );

		//added to debug
		m_debugObjPub = nh.advertise<geometry_msgs::PoseStamped>(
					"debug_object", 1, true);

		nh.param<double>( "max_resolution", max_resolution_, 0.0125 );
		nh.param<double>( "max_radius", max_radius_, 30.0 );

		nh.param<double>( "dist_dep_factor", dist_dep_, 0.005 );

		nh.param<std::string>( "map_folder", map_folder_, "." );

		nh.param<std::string>( "init_frame", init_frame_, "" );

		create_map_ = false;
		do_publish_tf_= true;

		responseId_ = -1;

    }
TaskScheduler::TaskScheduler(ros::NodeHandle & nh, boost::shared_ptr<TaskDefinition> tidle, double deftPeriod)
{
	aqid = 0;
	runScheduler = false;

	idle = tidle;
	idleTimeout = 0.001;

	tasks["idle"] = idle;
	defaultPeriod = deftPeriod;
	startingTime = ros::Time::now().toSec();

	mainThread.reset();
	pthread_mutex_init(&scheduler_mutex,NULL);
	pthread_cond_init(&scheduler_condition,NULL);
	pthread_mutex_init(&aqMutex,NULL);
	pthread_cond_init(&aqCond,NULL);

	ROS_INFO("Task scheduler created: debug %d\n",debug);

    n = nh;
    startTaskSrv = nh.advertiseService("start_task", &TaskScheduler::startTask,this);
    stopTaskSrv = nh.advertiseService("stop_task", &TaskScheduler::stopTask,this);
    getTaskListSrv = nh.advertiseService("get_all_tasks", &TaskScheduler::getTaskList,this);
    getTaskListLightSrv =nh.advertiseService("get_all_tasks_light", &TaskScheduler::getTaskListLight,this);
    getAllTaskStatusSrv = nh.advertiseService("get_all_status", &TaskScheduler::getAllTaskStatus,this);
    statusPub = nh.advertise<task_manager_msgs::TaskStatus>("status",20);
    keepAliveSub = nh.subscribe("keep_alive",1,&TaskScheduler::keepAliveCallback,this);
    lastKeepAlive = ros::Time::now();
}
        // Constructor
        NodeClass()
        {
			// initialization of variables
			is_initialized_bool_ = false;
			last_time_ = ros::Time::now();
			x_rob_m_ = 0.0;
			y_rob_m_ = 0.0;
			theta_rob_rad_ = 0.0;
			// set status of drive chain to WARN by default
			drive_chain_diagnostic_ = diagnostic_status_lookup_.WARN;
			
			// implementation of topics
            // published topics
			topic_pub_joint_state_cmd_ = n.advertise<sensor_msgs::JointState>("JointStateCmd", 1);
			topic_pub_odometry_ = n.advertise<nav_msgs::Odometry>("Odometry", 50);

            // subscribed topics
			topic_sub_CMD_pltf_twist_ = n.subscribe("cmd_vel", 1, &NodeClass::topicCallbackTwistCmd, this);
            topic_sub_EM_stop_state_ = n.subscribe("EMStopState", 1, &NodeClass::topicCallbackEMStop, this);
            topic_sub_drive_diagnostic_ = n.subscribe("Diagnostic", 1, &NodeClass::topicCallbackDiagnostic, this);
			//<diagnostic_msgs::DiagnosticStatus>("Diagnostic", 1);

            // implementation of service servers
            srv_server_init_ = n.advertiseService("Init", &NodeClass::srvCallbackInit, this);
            srv_server_reset_ = n.advertiseService("Reset", &NodeClass::srvCallbackReset, this);
            srv_server_shutdown_ = n.advertiseService("Shutdown", &NodeClass::srvCallbackShutdown, this);

			// implementation of service clients
            srv_client_get_joint_state_ = n.serviceClient<cob_srvs::GetJointState>("GetJointState");
        }
Exemplo n.º 8
0
  FootstepPlanner::FootstepPlanner(ros::NodeHandle& nh):
    as_(nh, nh.getNamespace(),
        boost::bind(&FootstepPlanner::planCB, this, _1), false)
  {
    srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (nh);
    typename dynamic_reconfigure::Server<Config>::CallbackType f =
      boost::bind (&FootstepPlanner::configCallback, this, _1, _2);
    srv_->setCallback (f);
    pub_text_ = nh.advertise<jsk_rviz_plugins::OverlayText>(
      "text", 1, true);
    pub_close_list_ = nh.advertise<sensor_msgs::PointCloud2>(
      "close_list", 1, true);
    pub_open_list_ = nh.advertise<sensor_msgs::PointCloud2>(
      "open_list", 1, true);
    srv_project_footprint_ = nh.advertiseService(
      "project_footprint", &FootstepPlanner::projectFootPrintService, this);
    srv_project_footprint_with_local_search_ = nh.advertiseService(
      "project_footprint_with_local_search", &FootstepPlanner::projectFootPrintWithLocalSearchService, this);
    {
      boost::mutex::scoped_lock lock(mutex_);
      if (!readSuccessors(nh)) {
        return;
      }

      JSK_ROS_INFO("building graph");
      buildGraph();
      JSK_ROS_INFO("build graph done");
    }
    sub_pointcloud_model_ = nh.subscribe("pointcloud_model", 1, &FootstepPlanner::pointcloudCallback, this);
    as_.start();
  }
  ///Constructor
  PowerCubeChainNode()
  {
  	n_ = ros::NodeHandle("~");

    pc_params_ = new PowerCubeCtrlParams();
    pc_ctrl_ = new PowerCubeCtrl(pc_params_);

    /// implementation of topics to publish
    topicPub_JointState_ = n_.advertise<sensor_msgs::JointState> ("/joint_states", 1);
    topicPub_ControllerState_ = n_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState> ("state", 1);
    topicPub_OperationMode_ = n_.advertise<std_msgs::String> ("current_operationmode", 1);
    topicPub_Diagnostic_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);

    /// implementation of topics to subscribe
    topicSub_CommandPos_ = n_.subscribe("command_pos", 1, &PowerCubeChainNode::topicCallback_CommandPos, this);
    topicSub_CommandVel_ = n_.subscribe("command_vel", 1, &PowerCubeChainNode::topicCallback_CommandVel, this);

    /// implementation of service servers
    srvServer_Init_ = n_.advertiseService("init", &PowerCubeChainNode::srvCallback_Init, this);
    srvServer_Stop_ = n_.advertiseService("stop", &PowerCubeChainNode::srvCallback_Stop, this);
    srvServer_Recover_ = n_.advertiseService("recover", &PowerCubeChainNode::srvCallback_Recover, this);
    srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &PowerCubeChainNode::srvCallback_SetOperationMode, this);

    initialized_ = false;
    stopped_ = true;
    error_ = false;
    last_publish_time_ = ros::Time::now();
  }
CloudMatcher::CloudMatcher(ros::NodeHandle& n):
	n(n),
	serviceMatchClouds(n.advertiseService("match_clouds", &CloudMatcher::match, this)),
	serviceMatchCloudsWithGuess(n.advertiseService("match_clouds_with_guess", &CloudMatcher::matchWithGuess, this))
{
	// load config
	string configFileName;
	if (ros::param::get("~config", configFileName))
	{
		ifstream ifs(configFileName.c_str());
		if (ifs.good())
		{
			icp.loadFromYaml(ifs);
		}
		else
		{
			ROS_ERROR_STREAM("Cannot load config from YAML file " << configFileName);
			icp.setDefault();
		}
	}
	else
	{
		ROS_WARN_STREAM("No config file specified, using default ICP chain.");
		icp.setDefault();
	}
	
	// replace logger
	if (getParam<bool>("useROSLogger", false))
		PointMatcherSupport::setLogger(new PointMatcherSupport::ROSLogger);
}
Exemplo n.º 11
0
EventsGenerator::EventsGenerator(ros::NodeHandle& n, ros::NodeHandle& np)
{
	double p;
	np.param("exp_timeout", p, 0.5);
	exp_timeout_ = ros::Duration(p);

	sub_desires_ = n.subscribe("desires_set", 1,
			&EventsGenerator::desiresCB, this);
	sub_intention_ = n.subscribe("intention", 1,
			&EventsGenerator::intentionCB, this);
	srv_cem_ = n.advertiseService("create_exploitation_matcher",
			&EventsGenerator::cemCB, this);
	srv_ctem_ = n.advertiseService("create_topic_exploitation_matcher",
			&EventsGenerator::ctemCB, this);

	pub_events_ = n.advertise<hbba_msgs::Event>("events", 100);

	ros::NodeHandle rosgraph_np(np, "rosgraph_monitor");
	rosgraph_monitor_.reset(new RosgraphMonitor(n, rosgraph_np));
	rosgraph_monitor_->registerCB(&EventsGenerator::rosgraphEventsCB, this);

	timer_ = n.createTimer(ros::Duration(1.0), &EventsGenerator::timerCB, this);

    parseExploitationMatches(n);
}
Exemplo n.º 12
0
	CaptureServer() :
			nh_private("~") {

		ROS_INFO("Creating localization");

		queue_size_ = 5;

		odom_pub = nh_.advertise<nav_msgs::Odometry>("vo", queue_size_);
		keyframe_pub = nh_.advertise<rm_localization::Keyframe>("keyframe",
				queue_size_);

		update_map_service = nh_.advertiseService("update_map",
				&CaptureServer::update_map, this);

		send_all_keyframes_service = nh_.advertiseService("send_all_keyframes",
				&CaptureServer::send_all_keyframes, this);

		clear_keyframes_service = nh_.advertiseService("clear_keyframes",
				&CaptureServer::clear_keyframes, this);

		rgb_sub.subscribe(nh_, "rgb/image_raw", queue_size_);
		depth_sub.subscribe(nh_, "depth/image_raw", queue_size_);
		info_sub.subscribe(nh_, "rgb/camera_info", queue_size_);

		// Synchronize inputs.
		sync.reset(
				new Synchronizer(SyncPolicy(queue_size_), rgb_sub, depth_sub,
						info_sub));

		sync->registerCallback(
				boost::bind(&CaptureServer::RGBDCallback, this, _1, _2, _3));

	}
Exemplo n.º 13
0
  ShooterVision() : nh_(ros::this_node::getName()), it_(nh_) {
    nh_.param<bool>("auto_start", active, false);
    vision.reset(new GrayscaleContour(nh_));
    vision->init();
    nh_.param<std::string>("symbol_camera", camera_topic,
                           "/right/right/image_raw");
    std::string get_shapes_topic;
    nh_.param<std::string>("get_shapes_topic",get_shapes_topic,"get_shapes");
    runService = nh_.advertiseService(get_shapes_topic+"/switch", &ShooterVision::runCallback, this);
    roiService = nh_.advertiseService("setROI",
                                      &ShooterVision::roiServiceCallback, this);
    //#ifdef DO_DEBUG
    // DebugWindow::init();
    //#endif
    foundShapesPublisher = nh_.advertise<navigator_msgs::DockShapes>(
        "found_shapes", 1000);
    image_sub_ = it_.subscribe(camera_topic, 1, &ShooterVision::run, this);

    int x_offset, y_offset, width, height;
    nh_.param<int>("roi/x_offset", x_offset, 73);
    nh_.param<int>("roi/y_offset", y_offset, 103);
    nh_.param<int>("roi/width", width, 499);
    nh_.param<int>("roi/height", height, 243);
    nh_.param<int>("size/height", size.height, 243);
    nh_.param<int>("size/width", size.width, 243);
    roi = Rect(x_offset, y_offset, width, height);
    TrackedShape::init(nh_);
    tracker.init(nh_);
  }
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;
}
Exemplo n.º 15
0
 void init()
 {
   nh.initNode();
   //~ nh.subscribe(sub);
   nh.advertiseService(fireService);
   nh.advertiseService(cancelService);
   nh.advertiseService(manualService);
 }
Exemplo n.º 16
0
T11::T11(ros::NodeHandle node) {
  knowledge_pub = node.advertise<t11_kb_modeling::Knowledge>(TOPIC_KB, 100);
  position_sub = node.subscribe(TOPIC_ROBOT_LOCATION, 100, &T11::positionCallback, this);
  hri_feature_sub = node.subscribe("t31_feature", 10, &T11::hriFeatureCallback, this);
  env_feature_sub = node.subscribe("t21_feature", 10, &T11::envFeatureCallback, this);
  service_get_location = node.advertiseService(SERVICE_GET_LOCATION, &T11::getLocation, this);
  service_get_all_sites = node.advertiseService(SERVICE_GET_ALL_SITES, &T11::getAllSites, this);
}
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;
}
Action_client_cmd_interface::Action_client_cmd_interface(ros::NodeHandle    &nh,
                                             ac::Kuka_action_client         &kuka_action_client,
                                             const std::string              &action_service_name,
                                             const std::string              &cmd_service_name)
    :
    nh(nh),
    kuka_action_client(kuka_action_client),
    curr_action_state(action_states::PENDING)
{
    action_service             = nh.advertiseService(action_service_name,&Action_client_cmd_interface::action_service_callback,this);
    cmd_interface_service      = nh.advertiseService(cmd_service_name,&Action_client_cmd_interface::cmd_interface_callback,this);
}
Exemplo n.º 19
0
 SayAction(std::string name) :
   as_(nh_, name, boost::bind(&SayAction::as_cb, this, _1), false),
   action_name_(name)
 {
   as_.start();
   srvServer_ = nh_.advertiseService("/say", &SayAction::service_cb, this);
   srvServer_mute_ = nh_.advertiseService("mute", &SayAction::service_cb_mute, this);
   srvServer_unmute_ = nh_.advertiseService("unmute", &SayAction::service_cb_unmute, this);
   sub_ = nh_.subscribe("/say", 1000, &SayAction::topic_cb, this);
   topicPub_Diagnostic_ = nh_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
   mute_ = false;
 }
Exemplo n.º 20
0
void BaseStation::initHandlers(ros::NodeHandle & node) {
    getObstaclesPositionService = node.advertiseService("basestation/getObstaclesPosition", &BaseStation::getObstaclesPosition, this);
    findRobotPositionAndAngleService = node.advertiseService("basestation/findRobotPositionAndAngle", &BaseStation::findRobotPositionAndAngle, this);
    showSolvedSudocubeService = node.advertiseService("basestation/showSolvedSudocube", &BaseStation::showSolvedSudocube, this);
    traceRealTrajectoryService = node.advertiseService("basestation/traceRealTrajectory", &BaseStation::traceRealTrajectory, this);
    loopEndedService = node.advertiseService("basestation/loopEnded", &BaseStation::loopEnded, this);

    startLoopClient = node.serviceClient<kinocto::StartLoop>("kinocto/startLoop");
    setRobotPositionAndAngleClient = node.serviceClient<kinocto::SetRobotPositionAndAngle>("kinocto/setRobotPositionAndAngle");

    updateRobotSubscriber = node.subscribe("basestation/updateRobotPosition", 50, &BaseStation::updateRobotPosition, this);
}
Exemplo n.º 21
0
bool ForceTorqueNode::init()
{
  m_isInitialized = false;
  //topicPub_ForceData_ = nh_.advertise<std_msgs::Float32MultiArray>("/arm_controller/wrench", 100);
  topicPub_ForceData_ = nh_.advertise<geometry_msgs::WrenchStamped>("/arm_controller/wrench", 100);
  //topicPub_ForceDataBase_ = nh_.advertise<std_msgs::Float32MultiArray>("/arm_controller/wrench_bl", 100);
  topicPub_Marker_ = nh_.advertise<visualization_msgs::Marker>("/visualization_marker", 1);
  srvServer_Init_ = nh_.advertiseService("/ft_controller/init", &ForceTorqueNode::srvCallback_Init, this);
  srvServer_Calibrate_ = nh_.advertiseService("/ft_controller/calibrate", &ForceTorqueNode::srvCallback_Calibrate, this);

  
  return true;
}
Exemplo n.º 22
0
CoaxVisionControl::CoaxVisionControl(ros::NodeHandle &node)
    :VisionFeedback(node), KF(node)

    ,reach_nav_state(node.serviceClient<coax_msgs::CoaxReachNavState>("reach_nav_state"))
    ,configure_comm(node.serviceClient<coax_msgs::CoaxConfigureComm>("configure_comm"))
    ,configure_control(node.serviceClient<coax_msgs::CoaxConfigureControl>("configure_control"))
    ,set_timeout(node.serviceClient<coax_msgs::CoaxSetTimeout>("set_timeout"))
    ,vicon_state_sub(node.subscribe("coax_57/vicon/odom",1,&CoaxVisionControl::viconCallback,this))
    ,coax_state_sub(node.subscribe("state",1, &CoaxVisionControl::StateCallback,this))
    ,raw_control_pub(node.advertise<coax_msgs::CoaxRawControl>("rawcontrol",1))
    ,vision_control_pub(node.advertise<coax_msgs::CoaxControl>("visioncontrol",1))
    ,vicon_control_pub(node.advertise<coax_msgs::viconControl>("vicon_control",1))
    ,vicon_state_pub(node.advertise<coax_msgs::viconState>("vicon_state",1))
    ,LOW_POWER_DETECTED(false)
    ,CONTROL_MODE(CONTROL_LANDED)
    ,FIRST_START(false)
    ,FIRST_STATE(true)
    ,FIRST_LANDING(false)
    ,FIRST_HOVER(false)
    ,INIT_DESIRE(false)
    ,INIT_IMU(false)
    ,coax_nav_mode(0)
    ,coax_control_mode(0)
    ,coax_state_age(0)
    ,raw_control_age(0)
    ,init_count(0)
    ,init_imu_count(0)
    ,rotor_ready_count(-1)
    ,last_state_time(0.0)
    ,rpy(0,0,0), accel(0,0,0), gyro(0,0,0), rpyt_rc(0,0,0,0), rpyt_rc_trim(0,0,0,0)
    ,range_al(0.0)
    ,pos_z(0.0),vel_z(0.0)
    ,motor_up(0),motor_lo(0)
    ,servo_roll(0),servo_pitch(0)
    ,roll_trim(0),pitch_trim(0)
    ,motor1_des(0.0),motor2_des(0.0),servo1_des(0.0),servo2_des(0.0)
    ,des_yaw(0.0),yaw_rate_des(0.0)
    ,roll_des(0.0),roll_rate_des(0.0)
    ,pitch_des(0.0),pitch_rate_des(0.0)
    ,altitude_des(0.0)
    ,stage(0),initial_pos(0),des_pos_x(0.0),des_pos_y(0.0),des_pos_z(0.0)
    ,des_pos_x_origin(0.0),des_pos_y_origin(0.0)
{
//	vicon_state_sub=node.subscribe("coax_57/vicon/odom",1,&CoaxVisionControl::viconCallback,this);
    set_nav_mode.push_back(node.advertiseService("set_nav_mode", &CoaxVisionControl::setNavMode, this));
    set_control_mode.push_back(node.advertiseService("set_control_mode", &CoaxVisionControl::setControlMode, this));

}
Exemplo n.º 23
0
Peg_replay::Peg_replay(ros::NodeHandle& node,
                       const std::string& plug_topic,
                       const std::string& socket_topic):
broadcaster_plug_link("world",plug_topic),broadcaster_socket("world",socket_topic),
index_subscriber(node,"index_sub")
{

    bPlayTraj       = false;
    bInit           = false;
    bListen_index   = false;
    service         = node.advertiseService("peg_replay_cmd", &Peg_replay::cmd_callback,this);
    ptr_node        = &node;

    cmds["load"]    = LOAD;
    cmds["start"]   = START;
    cmds["stop"]    = STOP;
    cmds["init"]    = INIT;
    cmds["listen"]  = LIST_INDEX;

    position_socket.setValue(0,0,0);
    plug_link_pos.setValue(0.03,-0.04,0);
    plug_link_rot.setRPY(0,0,M_PI);

    run_trajectories.set_initial_position(plug_link_pos,plug_link_rot);

    t      = 0;
    t_stop = 0;
}
Exemplo n.º 24
0
  SceneSegmentation()
  : nh_("~"), it_(nh_)
  {
    nh_.param<float>("sigma", sigma_, 0.5f);
    nh_.param<float>("k", k_, 500.f);
    nh_.param<int>("min", min_, 20);

    nh_.param<bool>("half", half_, true);
    bool draw_rect;
    nh_.param<bool>("draw_rect", draw_rect, true);

    ROS_INFO("Waiting for message on camera info topic.");
    camera_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>("camera_info", nh_);

    // segmentor_ = Segmentor::Ptr(new Segmentor(camera_info_->width, camera_info_->height, half_));
    if(half_)
      segmentor_ = Segmentor::Ptr(new Segmentor(camera_info_->width/2, camera_info_->height/2, draw_rect));
    else
      segmentor_ = Segmentor::Ptr(new Segmentor(camera_info_->width, camera_info_->height, draw_rect));

    srv_segment_ = nh_.advertiseService("segment_image", &SceneSegmentation::segment_image_cb, this);
    ROS_INFO("segment_image is ready.");

    sub_image_ = it_.subscribe("image_in", 1, &SceneSegmentation::image_cb, this);
    pub_image_ = it_.advertise("image_out", 1);
    pub_rects_ = nh_.advertise<duckietown_msgs::Rects>("rects_out", 1);
    pub_segments_ = nh_.advertise<duckietown_msgs::SceneSegments>("segments_out", 1);
  }
  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;
  }
  JointsClient()
  {
    //ros::Rate loop(10);
    srv = n.serviceClient<humans_msgs::HumanImgSrv>("okao_srv");

    viz_pub 
      = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);

    pps_pub 
      = n.advertise<humans_msgs::PersonPoseImgArray>("/v_human", 10);

    joints_last
      = n.advertiseService("joints_last_srv",
			   &JointsClient::jointsLastRequest, this);
    /*
    okao_id.push_back(1);
    okao_id.push_back(2);
    okao_id.push_back(3);
    okao_id.push_back(4);
    okao_id.push_back(5);
    okao_id.push_back(6);
    okao_id.push_back(7);
    okao_id.push_back(8);
    okao_id.push_back(9);
    okao_id.push_back(10);
    okao_id.push_back(11);
    okao_id.push_back(12);
    okao_id.push_back(13);
    */
    //joints_stream
    //  = n.advertiseService("joints_stream_srv",
    //			   &JointsClient::JointsStreamRequest, this);
    //okaoStack
    //  = n.serviceClient<okao_client::OkaoStack>("stack_send");
  }
Exemplo n.º 27
0
  IKTrajectoryExecutor()
  {

    //tell the joint trajectory action client that we want 
    //to spin a thread by default
    action_client = new TrajClient("joint_trajectory_action", true);

    //wait for the action server to come up
    while(ros::ok() && !action_client->waitForServer(ros::Duration(5.0)))
    {
      ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
    }

    //register a service to input desired Cartesian trajectories
    service = node.advertiseService("execute_cartesian_ik_trajectory", 
        &IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);

    //have to specify the order of the joints we're sending in our 
    //joint trajectory goal, even if they're already on the param server
    goal.trajectory.joint_names.push_back("shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("elbow_joint");
    goal.trajectory.joint_names.push_back("wrist_1_joint");
    goal.trajectory.joint_names.push_back("wrist_2_joint");
    goal.trajectory.joint_names.push_back("wrist_3_joint");

  }
Exemplo n.º 28
0
Sensor_manager::Sensor_manager(ros::NodeHandle& nh, wobj::WrapObject& wrapped_objects, obj::Socket_one &socket_one):
wrapped_objects(wrapped_objects),socket_one(socket_one)
{
    t_sensor       = psm::NONE;
    service_server = nh.advertiseService("sensor_manager_cmd",&Sensor_manager::sensor_manager_callback,this);
    initialise();
}
Exemplo n.º 29
0
ObjectRecognition::ObjectRecognition(ros::NodeHandle node_handle) :
    node_handle(node_handle),
    point_cloud_subscriber(node_handle, "/camera/depth_registered/points", 1),
    camera_info_subscriber(node_handle, "/camera/rgb/camera_info", 1),
    rgbd_image_synchronizer(RgbdImagePolicy(10), point_cloud_subscriber, camera_info_subscriber)
{

    boost::shared_ptr<std::vector<Object>> trained_objects(new std::vector<Object>);
    this->loadModels(trained_objects);
    this->object_pipeline = boost::shared_ptr<ObjectPipeline>(new ObjectPipeline(trained_objects));

    this->objects_publisher = node_handle.advertise<sepanta_msgs::Objects>("/object_recognition/objects", 5);
    this->turnOnService = node_handle.advertiseService("/object_recognition/turn_on", &ObjectRecognition::turnOn, this);
    this->turnOffService = node_handle.advertiseService("/object_recognition/turn_off", &ObjectRecognition::turnOff, this);

}
Exemplo n.º 30
0
	CoaxInterface(ros::NodeHandle & n)
	{
		reach_nav_state = n.serviceClient<coax_msgs::CoaxReachNavState>("reach_nav_state");
		configure_comm = n.serviceClient<coax_msgs::CoaxConfigureComm>("configure_comm");
		set_timeout = n.serviceClient<coax_msgs::CoaxSetTimeout>("set_timeout");
		
		raw_control_pub = n.advertise<coax_msgs::CoaxRawControl>("rawcontrol",1);
		coax_info_pub = n.advertise<geometry_msgs::Quaternion>("info",1);
		control_mode_pub = n.advertise<geometry_msgs::Quaternion>("control_mode",1);
		
		coax_state_sub = n.subscribe("state", 10, &CoaxInterface::coaxStateCallback, this);
		matlab_trim_sub = n.subscribe("trim", 10, &CoaxInterface::matlabTrimCallback, this);
		matlab_nav_mode_sub = n.subscribe("nav_mode", 10, &CoaxInterface::matlabNavModeCallback, this);
		matlab_raw_control_sub = n.subscribe("raw_control", 10, &CoaxInterface::matlabRawControlCallback, this);
		
		set_control_mode.push_back(n.advertiseService("set_control_mode", &CoaxInterface::setControlMode, this));
		
		roll_trim = 0;
		pitch_trim = 0;
		motor1 = 0;
		motor2 = 0;
		servo1 = 0;
		servo2 = 0;
		matlab_rawcontrol_age = 0;
		coax_state_age = 0;
		desired_nav_mode = 0;
	}