VrpnClientRos::VrpnClientRos(ros::NodeHandle nh, ros::NodeHandle private_nh)
{
output_nh_ = private_nh;
host_ = getHostStringFromParams(private_nh);
ROS_INFO_STREAM("Connecting to VRPN server at " << host_);
connection_ = std::shared_ptr<vrpn_Connection>(vrpn_get_connection_by_name(host_.c_str()));
ROS_INFO("Connection established");
double update_frequency;
private_nh.param<double>("update_frequency", update_frequency, 100.0);
mainloop_timer = nh.createTimer(ros::Duration(1 / update_frequency), boost::bind(&VrpnClientRos::mainloop, this));
double refresh_tracker_frequency;
private_nh.param<double>("refresh_tracker_frequency", refresh_tracker_frequency, 0.0);
if (refresh_tracker_frequency > 0.0)
{
refresh_tracker_timer_ = nh.createTimer(ros::Duration(1 / refresh_tracker_frequency),
boost::bind(&VrpnClientRos::updateTrackers, this));
}
std::vector<std::string> param_tracker_names_;
if (private_nh.getParam("trackers", param_tracker_names_))
{
for (std::vector<std::string>::iterator it = param_tracker_names_.begin();
it != param_tracker_names_.end(); ++it)
{
trackers_.insert(std::make_pair(*it, std::make_shared<VrpnTrackerRos>(*it, connection_, output_nh_)));
}
}
}
// If artificial switching, this method is called at set intervals (according to delTon, delToff) to toggle the estimator
void switchingTimerCB(const ros::TimerEvent& event)
{
if (estimatorOn)
{
estimatorOn = false;
switchingTimer = nh.createTimer(ros::Duration(delToff),&EKF::switchingTimerCB,this,true);
}
else
{
estimatorOn = true;
switchingTimer = nh.createTimer(ros::Duration(delTon),&EKF::switchingTimerCB,this,true);
}
}
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();
}
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);
}
AngularRateControl() : nh_("~"),lastIMU_(0.0),
lastCommand_(0.0), timeout_(1.5),
joy_control_("/teleop/twistCommand"), auto_control_("/mux/safeCommand"),
manual_override_(false), rc_override_(true)
{
nh_.param("kp",kp_,1.0);
nh_.param("ki",ki_,0.0);
nh_.param("kd",kd_,0.0);
nh_.param("imax",imax_,1E9);
nh_.param("period",period_,0.05);
nh_.param("motor_zero",motor_zero_,0.0);
nh_.param("input_scale",input_scale_,1.0);
double dTimeout;
nh_.param("timeout",dTimeout,1.5);
timeout_ = ros::Duration(dTimeout);
Pid_.initPid(kp_,ki_,kd_,imax_,-imax_);
// Make sure TF is ready
ros::Duration(0.5).sleep();
// Subscribe to the velocity command and setup the motor publisher
imu_sub_ = nh_.subscribe("imu",1,&AngularRateControl::imu_callback,this);
cmd_sub_ = nh_.subscribe("command",1,&AngularRateControl::cmd_callback,this);
mux_sub_ = nh_.subscribe("/mux/selected",1,&AngularRateControl::mux_callback,this);
rc_sub_ = nh_.subscribe("/sense",1,&AngularRateControl::rc_callback,this);
motor_pub_ = nh_.advertise<geometry_msgs::Twist>("motors",1);
debug_pub_ = nh_.advertise<geometry_msgs::Point>("debug",1);
//Create a callback for the PID loop
pid_loop_ = nh_.createTimer(ros::Duration(period_), &AngularRateControl::do_pid, this);
f_ = boost::bind(&AngularRateControl::config_callback, this, _1, _2);
server_.setCallback(f_);
}
Experiment::Experiment(ros::NodeHandle n):n_(n)
{
ros::NodeHandle n_private("~");
n_private.param("num_reps", num_reps_, 1000);
n_private.param("freq", freq_, 1.0);
n_private.param("start_x", start_x_, -5.0);
n_private.param("start_y", start_y_, 5.0);
n_private.param("goal_radius", radius_, 0.5);
n_private.param("learn", learn_, true);
n_private.param("learning_rate", learning_rate_, 0.5);
n_private.param("discount_factor", discount_factor_, 0.5);
n_private.param("max_explore", max_explore_, 8);
cnt_rep_ = 0;
move_stopped_ = false;
mode_ = MODE_REP_START;
states_ = new States(n);
actions_ = new Actions(n);
qobj_ = new QLearner(actions_->GetNumActions(),
states_->GetNumStates(), learning_rate_, discount_factor_, learn_,
max_explore_);
bool_sub_ = n.subscribe("/move_done", 1, &Experiment::bool_cb, this);
// ros::Subscriber odom_sub_ = n.subscribe("/odom", 10, odom_cb);
client_ = n.serviceClient<stage_light_ml::move_robot>("/move_robot");
timer_ = n.createTimer(ros::Duration(1/freq_), &Experiment::timer_cb, this);
}
GenericDiagnostic(std::string diagnosticName):nh() {
_updater.add(diagnosticName,this, &GenericDiagnostic::nodeDiagnostics);
_timer = nh.createTimer(ros::Duration(4),&GenericDiagnostic::diagnosticTimerCallback,this);
_updater.setHardwareID("none");
};
Node()
: private_nh("~"),
kill_listener(boost::bind(&Node::killed_callback, this)),
actionserver(nh, "moveto", false),
disabled(false) {
fixed_frame = uf_common::getParam<std::string>(private_nh, "fixed_frame");
body_frame = uf_common::getParam<std::string>(private_nh, "body_frame");
limits.vmin_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "vmin_b");
limits.vmax_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "vmax_b");
limits.amin_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "amin_b");
limits.amax_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "amax_b");
limits.arevoffset_b = uf_common::getParam<Eigen::Vector3d>(private_nh, "arevoffset_b");
limits.umax_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "umax_b");
traj_dt = uf_common::getParam<ros::Duration>(private_nh, "traj_dt", ros::Duration(0.0001));
odom_sub = nh.subscribe<Odometry>("odom", 1, boost::bind(&Node::odom_callback, this, _1));
trajectory_pub = nh.advertise<PoseTwistStamped>("trajectory", 1);
waypoint_pose_pub = private_nh.advertise<PoseStamped>("waypoint", 1);
update_timer =
nh.createTimer(ros::Duration(1. / 50), boost::bind(&Node::timer_callback, this, _1));
actionserver.start();
set_disabled_service = private_nh.advertiseService<SetDisabledRequest, SetDisabledResponse>(
"set_disabled", boost::bind(&Node::set_disabled, this, _1, _2));
}
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));
}
/*!
* \brief Initializes node to get parameters, subscribe and publish to topics.
*/
bool init()
{
// implementation of topics to publish
nh_.param("dsadevicestring", dsadevicestring_, std::string(""));
if (dsadevicestring_.empty()) return false;
nh_.param("dsadevicenum", dsadevicenum_, 0);
nh_.param("maxerror", maxerror_, 8);
double publish_frequency, diag_frequency;
nh_.param("debug", debug_, false);
nh_.param("polling", polling_, false);
nh_.param("use_rle", use_rle_, true);
nh_.param("diag_frequency", diag_frequency, 5.0);
frequency_ = 30.0;
if(polling_) nh_.param("poll_frequency", frequency_, 5.0);
nh_.param("publish_frequency", publish_frequency, 0.0);
auto_publish_ = true;
if(polling_){
timer_dsa = nh_.createTimer(ros::Rate(frequency_).expectedCycleTime(),boost::bind(&DsaNode::pollDsa, this));
}else{
timer_dsa = nh_.createTimer(ros::Rate(frequency_*2.0).expectedCycleTime(),boost::bind(&DsaNode::readDsaFrame, this));
if(publish_frequency > 0.0){
auto_publish_ = false;
timer_publish = nh_.createTimer(ros::Rate(publish_frequency).expectedCycleTime(),boost::bind(&DsaNode::publishTactileData, this));
}
}
timer_diag = nh_.createTimer(ros::Rate(diag_frequency).expectedCycleTime(),boost::bind(&DsaNode::publishDiagnostics, this));
if(!read_vector(nh_, "dsa_reorder", dsa_reorder_)){
dsa_reorder_.resize(6);
dsa_reorder_[0] = 2; // t1
dsa_reorder_[1] = 3; // t2
dsa_reorder_[2] = 4; // f11
dsa_reorder_[3] = 5; // f12
dsa_reorder_[4] = 0; // f21
dsa_reorder_[5] = 1; // f22
}
return true;
}
/****************************************************
* @brief : Default constructor
* @param : ros node handler
****************************************************/
RobotObserver(ros::NodeHandle& node)
{
node_=node;
// Getting robot's id from ros param
if(node_.hasParam("my_robot_id"))
{
node_.getParam("my_robot_id",my_id_);
} else {
my_id_="PR2_ROBOT";
}
waiting_timer_ = node_.createTimer(ros::Duration(1.0), &RobotObserver::timerCallback, this, true);
timer_on_=false;
enable_event_=true;
task_started_=false;
// Advertise subscribers
fact_list_sub_ = node_.subscribe("/agent_monitor/factList", 1, &RobotObserver::factListCallback, this);
fact_area_list_sub_ = node_.subscribe("/area_manager/factList", 1, &RobotObserver::factListAreaCallback, this);
human_list_sub_ = node_.subscribe("/pdg/humanList", 1, &RobotObserver::humanListCallback, this);
object_list_sub_ = node_.subscribe("/pdg/objectList", 1, &RobotObserver::objectListCallback, this);
current_action_list_sub_ = node_.subscribe("/human_monitor/current_humans_action", 1, &RobotObserver::CurrentActionListCallback, this);
next_action_list_sub_ = node_.subscribe("/plan_executor/next_humans_actions", 1, &RobotObserver::NextActionListCallback, this);
// Advertise publishers
attention_vizu_pub_ = node_.advertise<geometry_msgs::PointStamped>("head_manager/attention_vizualisation", 5);
init_action_client_ = new InitActionClient_t("pr2motion/Init", true);
// Initialize action client for the action interface to the head controller
head_action_client_ = new HeadActionClient_t("pr2motion/Head_Move_Target", true);
// Connection to the pr2motion client
connect_port_srv_ = node_.serviceClient<pr2motion::connect_port>("pr2motion/connect_port");
head_stop_srv_ = node_.serviceClient<pr2motion::connect_port>("pr2motion/Head_Stop");
ROS_INFO("[robot_observer] Waiting for pr2motion action server to start.");
init_action_client_->waitForServer(); //will wait for infinite time
head_action_client_->waitForServer(); //will wait for infinite time
ROS_INFO("[robot_observer] pr2motion action server started.");
pr2motion::InitGoal goal_init;
init_action_client_->sendGoal(goal_init);
pr2motion::connect_port srv;
srv.request.local = "joint_state";
srv.request.remote = "joint_states";
if (!connect_port_srv_.call(srv)){
ROS_ERROR("[robot_observer] Failed to call service pr2motion/connect_port");
}
srv.request.local = "head_controller_state";
srv.request.remote = "/head_traj_controller/state";
if (!connect_port_srv_.call(srv)){
ROS_ERROR("[robot_observer] Failed to call service pr2motion/connect_port");
}
pr2motion::Z_Head_SetMinDuration srv_MinDuration;
srv_MinDuration.request.head_min_duration=0.6;
if(!ros::service::call("/pr2motion/Z_Head_SetMinDuration",srv_MinDuration))
ROS_ERROR("[robot_observer] Failed to call service /pr2motion/Z_Head_SetMinDuration");
state_machine_ = new ObserverStateMachine(boost::cref(this));
state_machine_->start();
ROS_INFO("[robot_observer] Starting state machine, node ready !");
}
void PlayerTracker::spin()
{
ros::Timer timer = node.createTimer(ros::Duration(1.0/RATE), &PlayerTracker::updateOdometry,this);
ros::Rate r(RATE);
while(ros::ok()) {
ros::spinOnce();
r.sleep();
}
}
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);
}
KeyboardNode() {
ROS_INFO("Starting Keyboard Node...");
// wait for the simulator reset service to become available:
ROS_DEBUG("Waiting for simulator_reset service to become available");
ros::service::waitForService("simulator_reset");
// create a service client
reset_client = n_.serviceClient<std_srvs::Empty>("simulator_reset");
timer = n_.createTimer(ros::Duration(0.02),
&KeyboardNode::timerCallback, this);
}
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);
}
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);
}
ClothoidMainNode(ros::NodeHandle _nh):nh(_nh){
lane_sub = nh.subscribe<clothoid_msgs::Lane>("lane", 1, &ClothoidMainNode::laneCallback, this);
light_sub = nh.subscribe<clothoid_msgs::LightStatus>("light", 1, &ClothoidMainNode::lightCallback, this);
sign_sub = nh.subscribe<clothoid_msgs::TrafficSign>("sign", 1, &ClothoidMainNode::signCallback, this);
ped_sub = nh.subscribe<clothoid_msgs::Pedestrian>("pedestrian", 1, &ClothoidMainNode::pedestrianCallback, this);
vehicle_sub = nh.subscribe<clothoid_msgs::VehicleDetectStatus>("vehicle", 1, &ClothoidMainNode::vehicleCallback, this);
rcg_pub = nh.advertise<clothoid_msgs::katech_KCAN>("recognition_signal",1);
speed_pub = nh.advertise<clothoid_msgs::katech_KCAN>("vehicle_speed",1);
timer = nh.createTimer(ros::Duration(0.001), &ClothoidMainNode::timerCallback, this);
ROS_INFO("clothoid_main has initialized. waiting for messages.");
}
/*!
* \brief Default contructor.
*
* This description is displayed lower in the doxygen as an extended description along with
* the above brief description.
*/
HapticNode(char* argv[]):
nh_("/")
, nh_pvt_("~")
, input_topic_("/cloud_in")
, output_topic_("/cloud_out")
//, dyn_srv(ros::NodeHandle("~config")),
{
got_first_cloud_ = false;
now_ = ros::Time::now() - ros::Duration(1.0);
// Clouds in and out
sub_cloud_ = nh_.subscribe<sensor_msgs::PointCloud2>
(input_topic_, 1, boost::bind(&HapticNode::cloudCallback,this, _1));
pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>(output_topic_, 1);
pub_marker_cloud_ = nh_.advertise<visualization_msgs::MarkerArray>("/marker_cloud_array", 1);
// Disparity images in and out:
sub_disparity_1_ = nh_.subscribe<stereo_msgs::DisparityImage>
("/disparity_1", 1, boost::bind(&HapticNode::disparityCallback, this, _1));
// Marker topics
pub_marker_ = nh_.advertise<visualization_msgs::Marker>("/markers", 100);
pub_marker_array_ = nh_.advertise<visualization_msgs::MarkerArray>("/markers_array", 1);
// String status topic
pub_status_ = nh_pvt_.advertise<std_msgs::String>("status", 10);
// Create timer callback for auto-running of algorithm
update_timer_ = nh_.createTimer(ros::Duration(0.03333), boost::bind(&HapticNode::timerCallback, this));
slow_update_timer_ = nh_.createTimer(ros::Duration(0.5), boost::bind(&HapticNode::slowTimerCallback, this));
// This server is for the user to adjust the algorithm weights
dyn_cb = boost::bind( &HapticNode::dynamicCallback, this, _1, _2 );
dyn_srv.setCallback(dyn_cb);
// Set up the chai world and device
initializeHaptics();
ROS_INFO("Finished constructor!");
}
GraphSlamNode::GraphSlamNode(ros::NodeHandle nh) :
_nh(nh),
graph("/map"),
place_recognizer_(new BinaryGistRecognizer()),
optimizer_(new G2oOptimizer()),
transformation_estimator_(new FeatureTransformationEstimator(boost::bind(&GraphSlamNode::newEdgeCallback, this, _1))),
projector_(new OccupancyGridProjector(nh)),
tfl(ros::Duration(10.0))
{
// Read odometry frames.
nh.param<std::string>("odom_frame", odom_frame_, "odom");
ROS_INFO("odom_frame: %s", odom_frame_.c_str());
nh.param<std::string>("odom_childframe", odom_childframe_, "base_link");
ROS_INFO("odom_childframe: %s", odom_childframe_.c_str());
current_node_ = "";
last_node_ = "";
most_recent_loop_closure_ = "";
current_odometry_pose_ = Eigen::Isometry3d::Identity();
last_odometry_pose_ = Eigen::Isometry3d::Identity();
odom_information_ = Eigen::MatrixXd::Zero(6,6);
map_transform_ = Eigen::Isometry3d::Identity();
odom_transform_.frame_id_ = "/map";
odom_transform_.child_frame_id_ = odom_frame_;
// Initialize publishers
pose_pub_ = _nh.advertise<geometry_msgs::PoseStamped>("graph_pose", 1);
graph_display_pub_ = _nh.advertise<graph_slam_msgs::Graph>("/display_graph", 1);
sensor_request_pub_ = _nh.advertise<graph_slam_msgs::SensorRequest>("/sensor_request", 1);
// Initialize synchronized sensor data subscriber.
sensor_sub_.subscribe(nh, "/sensor_data", 10);
sensor_sub_filter_ = new tf::MessageFilter<graph_slam_msgs::SensorDataArray>(sensor_sub_, tfl, odom_frame_, 10);
sensor_sub_filter_->registerCallback(boost::bind(&GraphSlamNode::sensorDataCallback, this, _1));
// Initialize timer callbacks.
odom_timer_ = nh.createTimer(ros::Duration(.1), &GraphSlamNode::odomTimerCallback, this);
optimization_timer_ = nh.createTimer(ros::Duration(5.), &GraphSlamNode::optimizationTimerCallback, this);
graph.addSensor("/base_link", Eigen::Isometry3d::Identity());
}
TimeredDiagnosticUpdater::TimeredDiagnosticUpdater(
ros::NodeHandle& nh,
const ros::Duration& timer_duration):
diagnostic_updater_(new diagnostic_updater::Updater)
{
timer_ = nh.createTimer(
timer_duration, boost::bind(
&TimeredDiagnosticUpdater::timerCallback,
this,
_1));
timer_.stop();
}
VelocitySmootherNode(): nh_(), pnh_("~") {
latest_time_ = ros::Time::now();
dyn_srv_ = boost::make_shared<dyn::Server<Config> >(pnh_);
dyn::Server<Config>::CallbackType f = boost::bind(&VelocitySmootherNode::dynConfigCallback, this, _1, _2);
dyn_srv_->setCallback(f);
pub_vel_ = boost::shared_ptr<geometry_msgs::Twist>(new geometry_msgs::Twist());
pub_ = nh_.advertise<geometry_msgs::Twist>("output", 100);
sub_ = nh_.subscribe("input", 1, &VelocitySmootherNode::velCallback, this);
timer_ = nh_.createTimer(timerDuration(), &VelocitySmootherNode::timerCallback, this);
};
CInfraRedsController::CInfraRedsController(std::string name, CQboduinoDriver *device_p, ros::NodeHandle& nh) : CController(name,device_p,nh)
{
ir0_=0;
ir1_=0;
ir2_=0;
std::string topic,irs_topic;
nh.param("controllers/"+name+"/topic", topic, std::string("cmd_mics"));
nh.param("controllers/"+name+"/irs_topic", irs_topic, std::string("irs_state"));
nh.param("controllers/"+name+"/rate", rate_, 1.0);
irs_pub_ = nh.advertise<qbo_arduqbo::Irs>(irs_topic, 1);
timer_=nh.createTimer(ros::Duration(1/rate_),&CInfraRedsController::timerCallback,this);
}
CMicsController::CMicsController(std::string name, CQboduinoDriver *device_p, ros::NodeHandle& nh) : CController(name,device_p,nh)
{
m0_=0;
m1_=0;
m2_=0;
std::string mics_topic;
nh.param("controllers/"+name+"/mics_topic", mics_topic, std::string("mics_state"));
nh.param("controllers/"+name+"/rate", rate_, 1.0);
mics_pub_ = nh.advertise<std_msgs::UInt16MultiArray>(mics_topic, 1);
timer_=nh.createTimer(ros::Duration(1/rate_),&CMicsController::timerCallback,this);
}
bool ROSnode::Prepare() {
Handle.param("/ar_tag_number", arTagNumber, 9);
Handle.param("/use_timer", withTimer, true);
Handle.param("/reset_time", resetTime, 120.0);
spottedTags = std::vector<int>(arTagNumber, 0);
totalTags = 0;
tagSub = Handle.subscribe("ar_pose_marker", 10, &ROSnode::tagCallback, this);
countPub = Handle.advertise<std_msgs::Int32>("/tag_count", 10);
resetService = Handle.advertiseService("/reset", &ROSnode::resetServiceCallback, this);
if(withTimer)
resetTimer = Handle.createTimer(ros::Duration(resetTime), &ROSnode::resetTimerCallback, this);
return true;
}
CalibrationRegression() : nh_("~") {
nh_.param<int>("input_sample",input_sample_,500);
nh_.param<double>("watchdog_interval",watchdog_interval_seconds,0.02);
// Make sure TF is ready
ros::Duration(0.5).sleep();
CHECK(StringToLinearSolverType(FLAGS_linear_solver,
&options.linear_solver_type));
CHECK(StringToPreconditionerType(FLAGS_preconditioner,
&options.preconditioner_type));
CHECK(StringToSparseLinearAlgebraLibraryType(
FLAGS_sparse_linear_algebra_library,
&options.sparse_linear_algebra_library_type));
options.num_linear_solver_threads = FLAGS_num_threads;
options.max_num_iterations = FLAGS_num_iterations;
options.minimizer_progress_to_stdout = true;
options.num_threads = FLAGS_num_threads;
options.eta = FLAGS_eta;
options.function_tolerance = 3e-7;
options.max_solver_time_in_seconds = FLAGS_max_solver_time;
options.use_nonmonotonic_steps = FLAGS_nonmonotonic_steps;
options.update_state_every_iteration = true;
quaternion_parameterization = new ceres::QuaternionParameterization;
CHECK(StringToTrustRegionStrategyType(FLAGS_trust_region_strategy,
&options.trust_region_strategy_type));
CHECK(StringToDoglegType(FLAGS_dogleg, &options.dogleg_type));
options.use_inner_iterations = FLAGS_inner_iterations;
arm_sub_ = nh_.subscribe("/output_arm_position",1,&CalibrationRegression::ArmPositionCallback,this);
tag_sub_ = nh_.subscribe("/output_tag_position",1,&CalibrationRegression::TagPositionCallback,this);
watchdog_timer = nh_.createTimer(ros::Duration(watchdog_interval_seconds), &CalibrationRegression::watchdog, this);
tag_avail = false;
arm_avail = false;
q2i[0] = 0; q2i[1] = 0; q2i[2] = 0; q2i[3] = 1;
p2i[0] = 0; p2i[1] = 0; p2i[2] = 0;
p1i[0] = 0; p1i[1] = 0; p1i[2] = 0;
pbar[0] = -0.011857; pbar[1] = -0.018233; pbar[2] = 0.166546;
q1[0] = 0.001397; q1[1] = -0.022436; q1[2] = 0.016104; q1[3] = -0.999611;
//t1[0] = 0.184323; t1[1] = 0.308063; t1[2] = 0.413035;
t1[0] = 0.184323; t1[1] = 0.413035;
count = 0;
ROS_INFO("CERES SOLVER IS READY TO USE");
}
AALExtensionAction::AALExtensionAction(ros::NodeHandle n):
hasGoal(false),
current_phase(FINISHED),
current_state(CONTRACTED),
as_(n,"/aal_extension",false),
last_joint_states_(NULL),
joint_names_(NULL)
{
//Initialize pointers
joint_names_ = new std::string[7];
last_joint_states_ = new map<std::string,sensor_msgs::JointState>();
//Initialize action server
as_.registerGoalCallback(boost::bind(&AALExtensionAction::goalCB, this, _1));
as_.registerCancelCallback(boost::bind(&AALExtensionAction::preemptCB, this, _1));
as_.start();
next_contracting_angles.push_back(CONTRACTING_JOINT_1);
next_contracting_angles.push_back(CONTRACTING_JOINT_2);
next_contracting_angles.push_back(CONTRACTING_JOINT_3);
next_contracting_angles.push_back(CONTRACTING_JOINT_4);
next_contracting_angles.push_back(CONTRACTING_JOINT_5);
next_contracting_angles.push_back(CONTRACTING_JOINT_6);
next_contracting_angles.push_back(CONTRACTING_JOINT_CLAW);
next_extending_angles.push_back(EXTENDING_JOINT_1);
next_extending_angles.push_back(EXTENDING_JOINT_2);
next_extending_angles.push_back(EXTENDING_JOINT_3);
next_extending_angles.push_back(EXTENDING_JOINT_4);
next_extending_angles.push_back(EXTENDING_JOINT_5);
next_extending_angles.push_back(EXTENDING_JOINT_6);
next_extending_angles.push_back(EXTENDING_JOINT_CLAW);
next_angles.resize(7);
next_angles[0] = next_contracting_angles[0];
next_angles[1] = next_contracting_angles[1];
next_angles[2] = next_contracting_angles[2];
next_angles[3] = next_contracting_angles[3];
next_angles[4] = next_contracting_angles[4];
next_angles[5] = next_contracting_angles[5];
next_angles[6] = next_contracting_angles[6];
received_goal = ros::Time::now();
//Initialize timer:
main_loop_timer = n.createTimer(ros::Duration(1/30.0),&AALExtensionAction::mainLoop,this);
main_loop_timer.start();
}
// Consstruct a new RandomWalk object and hook up this ROS node
// to the simulated robot's velocity control and laser topics
AFRL_Driver::AFRL_Driver(ros::NodeHandle& nh) {
PIDTimer = nh.createTimer(ros::Duration(0.1), &AFRL_Driver::PID_control, this);
commandPub = nh.advertise<geometry_msgs::Twist>("/mobile_base/commands/velocity", 1);
laserSub = nh.subscribe("/base_scan", 1, &AFRL_Driver::commandCallBack, this);
bumperSub = nh.subscribe("/mobile_base/events/bumper", 1, &AFRL_Driver::bumperCallBack, this);
commsSub = nh.subscribe("/drone_comms", 1, &AFRL_Driver::commsCallBack, this);
// commsPub = nh.advertise<std_msgs::UInt8>("/ground_comms", 1);
tagState = TAG_NULL;
this->error_sum = 0;
this->control = 0;
ros::spinOnce();
}
void TfLookup::start(ros::NodeHandle &n)
{
_srvLookupTransform = n.advertiseService("/lookupTransform",
&TfLookup::srvLookupTransform, this);
_al_server.reset(new AlServer(n, "tf_lookup",
boost::bind(&TfLookup::alGoalCb, this, _1), false));
_al_server->start();
_tf_streamer.start(n, boost::bind(&TfLookup::lookupTransform, this, _1, _2, _3, _4),
boost::bind(&tf::TransformListener::resolve, _tfListener.get(), _1));
_check_timer = n.createTimer(ros::Duration(5),
boost::bind(&TfLookup::periodicCheck, this, _1));
}
SpatialPerspectiveLevel2::SpatialPerspectiveLevel2(ros::NodeHandle &nh, const ros::NodeHandle &privNh) :
updateVisualizerTimer(nh.createTimer(ros::Duration(0.5), &SpatialPerspectiveLevel2::callbackTimer, this)),
opc(new wysiwyd::wrdac::OPCClient(ros::this_node::getName().substr(1)))
{
if (!yarp::os::Network::checkNetwork()) {
ROS_ERROR("YARP could not be initialized");
} else {
int itry=0;
while (!opc->connect("OPC") && itry<3) {
ROS_INFO("Waiting for connection to OPC...");
ros::Duration(0.5).sleep();
itry++;
}
}
}
void DiscretePolicyManager::Initialize( DiscretePolicyInterface::Ptr& interface,
ros::NodeHandle& nh,
ros::NodeHandle& ph )
{
_interface = interface;
_actionPub = ph.advertise<DiscreteAction>( "actions", 0 );
_paramSub = nh.subscribe( "param_updates",
1,
&DiscretePolicyManager::ParamCallback,
this );
ros::NodeHandle subh( ph.resolveName("input_streams") );
_inputStreams.Initialize( subh );
unsigned int inputDim = _inputStreams.GetDim();
unsigned int outputDim = _interface->GetOutputSizes().array().prod();
unsigned int numHiddenLayers, layerWidth;
GetParamRequired( ph, "network/num_hidden_layers", numHiddenLayers );
GetParamRequired( ph, "network/layer_width", layerWidth );
_network = std::make_shared<NetworkType>( inputDim,
outputDim,
numHiddenLayers,
layerWidth );
_network->SetInputSource( &_networkInput );
_networkParameters = _network->CreateParameters();
VectorType w = _networkParameters->GetParamsVec();
percepto::randomize_vector( w, -0.1, 0.1 );
_networkParameters->SetParamsVec( w );
if( HasParam( ph, "seed" ) )
{
int seed;
GetParam( ph, "seed", seed );
_engine.seed( seed );
}
else
{
boost::random::random_device rng;
_engine.seed( rng() );
}
double updateRate;
GetParamRequired( ph, "update_rate", updateRate );
_timer = ph.createTimer( ros::Duration( 1.0/updateRate ),
&DiscretePolicyManager::UpdateCallback,
this );
}
