void PinholeRGBDevice::createSubscribers(ros::NodeHandle & nh,
image_transport::ImageTransport & image_transport_nh,
const std::string & main_topic)
{
std::stringstream ss;
ss << main_topic << "/image";
image_sub_ = image_transport_nh.subscribe(ss.str(), 1, &PinholeRGBDevice::imageCallback, this);
ss.str("");
ss << main_topic << "/camera_info";
camera_info_sub_ = nh.subscribe(ss.str(), 1, &PinholeRGBDevice::cameraInfoCallback, this);
}
BehaviorBase() : nh("~") {
// scanSub = nh.subscribe("scans",1,&CollisionAvoidance::pc_callback,this);
scanSub = nh.subscribe("scans",1,&BehaviorBase::pc_callback,this);
velPub = nh.advertise<geometry_msgs::Twist>("vel_output",1);
nh.param("max_linear_speed",max_linear_speed, 0.0);
nh.param("max_angular_speed",max_angular_speed, 1.0);
nh.param("desired_range",desired_range, 1.0);
nh.param("k_r",k_r, 1.0);
nh.param("k_alpha",k_alpha, 1.0);
}
MyClass()
:pnh("~")
{
sub = nh.subscribe("/humans/kinect_v2", 1, &MyClass::callback, this);
pub = nh.advertise<std_msgs::String>("/pub_topic", 1);
pnh.param<std::string>("output", output_filename, "test.json");
out_file << output_filename << ".json";
joint_filename = "joint_index.txt";
init_table(joint_filename);
}
SpeechRecog()
{
speech_sub
= nh.subscribe("mecab_res", 1,
&SpeechRecog::speechCallback, this);
okao_sub
= nh.subscribe("/humans/RecogInfo", 1,
&SpeechRecog::okaoCallback, this);
word_srv
= nh.advertiseService("word_search",
&SpeechRecog::wordSearchCallback, this);
//recog_pub_ =
// nh.advertise<humans_msgs::Humans>("/humans/RecogInfo", 1);
//ファイルからメモリにこれまでの単語情報を書き込む機能
}
ExportVtk::ExportVtk(ros::NodeHandle& n):
n(n),
cloudTopic(getParam<string>("cloudTopic", "/static_point_cloud")),
mapFrame(getParam<string>("mapFrameId", "/map")),
recordOnce(getParam<bool>("recordOnce", "false")),
transformation(PM::get().TransformationRegistrar.create("RigidTransformation"))
{
// ROS initialization
cloudSub = n.subscribe(cloudTopic, 100, &ExportVtk::gotCloud, this);
// Parameters for 3D map
}
OculusDb(char choice) :
dbhost("192.168.4.170"),
dbuser("root"),
dbpass("tmsdb"),
dbname("rostmsdb"),
choice(choice)
{
//Init Vicon Stream
ROS_ASSERT(init_oculusdb());
// Subscriber for tms_db_data topic
data_sub = nh.subscribe("tms_db_data", 100, &OculusDb::ocMoveCallback, this);
}
ImageConverter()
: it_(nh_)
{
x = 0;
y = 0;
image_sub_ = it_.subscribe("image", 1, &ImageConverter::imageCb, this);
axis_sub = nh_.subscribe("input_joy", 1, &ImageConverter::axis, this);
cv::namedWindow(WINDOW);
}
Explore(std::string name):
as_(nh_, name, false),
action_name_(name),
ac_("move_base", true)
{
explore_state_ = STOP;
explore_ = false;
firstGoalSend = false;
as_.registerGoalCallback(boost::bind(&Explore::goalCB, this));
as_.registerPreemptCallback(boost::bind(&Explore::preemptCB, this));
as_.start();
alghoritm_state_sub_ = nh_.subscribe("alghoritm_state", 1, &Explore::alghoritmStateCallBack, this);
posSub = nh_.subscribe("/amcl_pose", 1, &Explore::poseCallback, this);
deadlock_service_state_sub = nh_.subscribe("/deadlock_service_state", 1, &Explore::deadlockServiceStateCb, this);
}
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);
}
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);
};
Odometry() {
q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
first_odo = true;
sub_odo = n.subscribe("input_odo", 1, &Odometry::getOdo, this);
sub_imu = n.subscribe("input_imu", 1, &Odometry::getImu, this);
pub_data = n.advertise<nav_msgs::Odometry>("output_pose", 1);
};
int main()
{
buzzer = 0;
led = 1;
nh.initNode();
nh.subscribe(sub);
while (1)
{
nh.spinOnce();
wait_ms(1);
}
}
void infoCallback(const sensor_msgs::CameraInfoConstPtr& info_msg)
{
if (calibrated)
return;
cam_model_.fromCameraInfo(info_msg);
pattern_detector_.setCameraMatrices(cam_model_.intrinsicMatrix(), cam_model_.distortionCoeffs());
calibrated = true;
image_sub_ = nh_.subscribe("/camera/rgb/image_mono", 1, &CalibrateKinectCheckerboard::imageCallback, this);
ROS_INFO("[calibrate] Got image info!");
}
void SwissRangerDevice::createSubscribers(ros::NodeHandle & nh,
image_transport::ImageTransport & image_transport_nh,
const std::string & main_topic)
{
std::stringstream ss;
ss << main_topic << "/cloud";
cloud_sub_ = nh.subscribe(ss.str(), 1, &SwissRangerDevice::cloudCallback, this);
ss.str("");
ss << main_topic << "/intensity";
camera_sub_ = image_transport_nh.subscribeCamera(ss.str(), 1, &SwissRangerDevice::imageCallback, this);
}
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;
}
BaseMotionController( ros::NodeHandle &n ,float x,float y, float roll,float pitch,float yaw):node_handler(n)
{
xval = x;
yval = y;
rollval=roll;
pitchval=pitch;
yawval = yaw;
// Velocity control for the YouBot base.
base_velocities_publisher = node_handler.advertise<geometry_msgs::Twist>( "/cmd_vel", 1 );
base_odom = node_handler.subscribe("/odom", 1, &BaseMotionController::OdomCallback, this);
odom_received = false;
}
// Constructeur
Communication_API_schneider::Communication_API_schneider(ros::NodeHandle noeud,const std::string num_API)
{
// Publishers
pub_sorties = noeud.advertise<automates::Sorties>("/automates/Sorties_ap"+num_API, 1);
// Subscriber
sub_entrees = noeud.subscribe("/automates/Entrees_ap"+num_API, 1, &Communication_API_schneider::Callback_Entrees_api,this);
entrees_api=0;
sorties_api=0;
}
beacon_robot_pose_estimate_ros() : np_("~")
{
f = boost::bind(&beacon_robot_pose_estimate_ros::configure_callback, this, _1, _2);
server.setCallback(f);
robot1_pose_ = n_.advertise<geometry_msgs::Pose2D>("robot1_pose", 1);
robot2_pose_ = n_.advertise<geometry_msgs::Pose2D>("robot2_pose", 1);
input_cloud_ = n_.subscribe("input_cloud", 1, &beacon_robot_pose_estimate_impl::topicCallback_input_cloud, &component_implementation_);
odom1_ = n_.subscribe("odom1", 1, &beacon_robot_pose_estimate_impl::topicCallback_odom1, &component_implementation_);
odom2_ = n_.subscribe("odom2", 1, &beacon_robot_pose_estimate_impl::topicCallback_odom2, &component_implementation_);
np_.param("x_robot1", component_config_.x_robot1, (double)0.0);
np_.param("y_robot1", component_config_.y_robot1, (double)0.0);
np_.param("x_robot2", component_config_.x_robot2, (double)0.0);
np_.param("y_robot2", component_config_.y_robot2, (double)0.0);
np_.param("world_link", component_config_.world_link, (std::string)"world");
np_.param("robot1_link", component_config_.robot1_link, (std::string)"robot1_base_link");
np_.param("robot2_link", component_config_.robot2_link, (std::string)"robot2_base_link");
np_.param("detection_distance", component_config_.detection_distance, (double)0.07);
}
setpoint_gains_pub() {
ros::NodeHandle params("~");
setpoint_subscriber_ = node_handle_.subscribe("setpoint", 1,
&setpoint_gains_pub::setpointCallback, this);
setpoint_publisher_ = node_handle_.advertise<geometry_msgs::Pose>(
"setpoint", 1);
// gains_subscriber_ = node_handle_.subscribe("gains", 1,
// &setpoint_gains_pub::gainsCallback, this);
// gains_publisher_ = node_handle_.advertise</*geometry_msgs::Pose*/>(
// "/gains", 1);
}
common_servo_converter_ros() : np_("~")
{
f = boost::bind(&common_servo_converter_ros::configure_callback, this, _1, _2);
server.setCallback(f);
output_ = n_.advertise<std_msgs::UInt16>("output", 1);
input_ = n_.subscribe("input", 1, &common_servo_converter_impl::topicCallback_input, &component_implementation_);
np_.param("inverted", component_config_.inverted, (int)0);
np_.param("offset", component_config_.offset, (int)0);
}
RPNavigationNode::RPNavigationNode(ros::NodeHandle& node) :
node(node),
obstacle_avoidance_driving_direction(STOP),
framing_driving_direction(STOP),
driving_direction_source(OBSTACLE_AVOIDANCE_NODE)
{
// Initialize driving direction publisher
driving_direction_publisher = node.advertise<std_msgs::UInt16>("/rp/navigation/driving_direction_and_source", 1);
// Initialize driving direction subscribers
obstacle_avoidance_subscriber = node.subscribe("/rp/obstacle_avoidance/driving_direction", 1, &RPNavigationNode::obstacleAvoidanceDirectionMessageCallback, this);
framing_subscriber = node.subscribe("/rp/framing/driving_direction", 1, &RPNavigationNode::framingDirectionMessageCallback, this);
// Initialize Command & Control subscriber
auton_photography_subscriber = node.subscribe("/rp/autonomous_photography/direction_source", 1, &RPNavigationNode::autonomousPhotographyDirectionSourceMessageCallback, this);
// Spin at 5 Hz
DrivingDirection driving_direction;
DirectionSource direction_source;
ros::Rate rate(5);
while (ros::ok())
{
getDrivingDirectionAndSource(&driving_direction, &direction_source);
// Publish the direction
ROS_INFO("Navigation direction: %s", (driving_direction == LEFT) ? "LEFT" :
(driving_direction == RIGHT) ? "RIGHT" :
(driving_direction == STOP) ? "STOP" :
(driving_direction == FORWARD) ? "FORWARD" : "NONE");
std_msgs::UInt16 converted_direction_and_source;
converted_direction_and_source.data = (drivingDirectionToUint8(driving_direction) << 8) | (directionSourceToUint8(direction_source));
driving_direction_publisher.publish(converted_direction_and_source);
ros::spinOnce();
rate.sleep();
}
};
GraspLocalizer::GraspLocalizer(ros::NodeHandle& node, const std::string& cloud_topic,
const std::string& cloud_frame, int cloud_type, const std::string& svm_file_name,
const Parameters& params)
: cloud_left_(new PointCloud()), cloud_right_(new PointCloud()),
cloud_frame_(cloud_frame), svm_file_name_(svm_file_name), num_clouds_(params.num_clouds_),
num_clouds_received_(0), size_left_(0)
{
// subscribe to input point cloud ROS topic
if (cloud_type == CLOUD_SIZED)
cloud_sub_ = node.subscribe(cloud_topic, 1, &GraspLocalizer::cloud_sized_callback, this);
else if (cloud_type == POINT_CLOUD_2)
cloud_sub_ = node.subscribe(cloud_topic, 1, &GraspLocalizer::cloud_callback, this);
// create ROS publisher for grasps
grasps_pub_ = node.advertise<agile_grasp::Grasps>("grasps", 10);
// create localization object and initialize its parameters
localization_ = new Localization(params.num_threads_, true, params.plotting_mode_);
localization_->setCameraTransforms(params.cam_tf_left_, params.cam_tf_right_);
localization_->setWorkspace(params.workspace_);
localization_->setNumSamples(params.num_samples_);
localization_->setFingerWidth(params.finger_width_);
localization_->setHandOuterDiameter(params.hand_outer_diameter_);
localization_->setHandDepth(params.hand_depth_);
localization_->setInitBite(params.init_bite_);
localization_->setHandHeight(params.hand_height_);
min_inliers_ = params.min_inliers_;
if (params.plotting_mode_ == 0)
{
plots_handles_ = false;
}
else
{
plots_handles_ = false;
if (params.plotting_mode_ == 2)
localization_->createVisualsPub(node, params.marker_lifetime_, cloud_frame_);
}
}
bebop_control()
{
// Parameters
ros::NodeHandle nhp("~");
nhp.param<bool>("lazy", lazy, false);
nhp.param<std::string>("target", target, "ugv0");
nhp.param<double>("radius", radius, 1.0);
nhp.param<double>("kp", kp, 0.5);
nhp.param<double>("kw", kw, 1.0);
nhp.param<double>("kpd", kpd, 0.5);
// Publishers
velCmdPub = nh.advertise<geometry_msgs::Twist>("/bebop/cmd_vel",1);
takeoffPub = nh.advertise<std_msgs::Empty>("bebop/takeoff",1);
landPub = nh.advertise<std_msgs::Empty>("bebop/land",1);
resetPub = nh.advertise<std_msgs::Empty>("bebop/reset",1);
// Initialize states
autonomy = false;
mocapOn = false;
bebopVelOn = false;
targetVelOn = false;
// Subscribers
joySub = nh.subscribe("joy",1,&bebop_control::joyCB,this);
poseSub = nh.subscribe("bebop/pose",1,&bebop_control::poseCB,this);
bebopVelSub = nh.subscribe("bebop/vel",1,&bebop_control::bebopVelCB,this);
target1VelSub = nh.subscribe("ugv0/vel",1,&bebop_control::targetVelCB,this);
target2VelSub = nh.subscribe("ugv1/vel",1,&bebop_control::targetVelCB,this);
// Warning message
while (!(ros::isShuttingDown()) and (!mocapOn or !bebopVelOn or !targetVelOn))
{
if (!mocapOn) { std::cout << "BEBOP POSE NOT PUBLISHED! THE WALL IS DOWN!\nIs the mocap on?" << std::endl; }
if (!bebopVelOn) { std::cout << "BEBOP VELOCITY NOT PUBLISHED! => Bad tracking performance\nIs velocity filter running?" << std::endl; }
if (!targetVelOn) { std::cout << "TARGET VELOCITY NOT PUBLISHED! => Bad tracking performance\nIs velocity filter running?" << std::endl; }
ros::spinOnce();
ros::Duration(0.1).sleep();
}
}
maze_navigator_node() : alpha(0.0175), alpha1(0.01), alpha_align(0.0195), v(.17), w(0), mode(STRAIGHT_FORWARD), prevmode(STRAIGHT_FORWARD), goalObject("red cube")
{
//pathClient = n_.serviceClient<mapper::PathToObject>("path_to_object");
followsPath = false;
hasIR = false;
leftBool1 = false;
rightBool1 = false;
leftBool2 = false;
rightBool2 = false;
n_ = ros::NodeHandle("~");
ir_reader_subscriber_ = n_.subscribe("/ir_reader_node/cdistance", 1, &maze_navigator_node::irCallback, this);
encoders_subscriber_ = n_.subscribe("/arduino/encoders", 1, &maze_navigator_node::encodersCallback, this);
imu_subscriber_ = n_.subscribe("/imu_angle", 1, &maze_navigator_node::imuAngleCallback, this);
odometry_subscriber_ = n_.subscribe("/posori/Twist", 1, &maze_navigator_node::odometryCallback, this);
pathToUnknownSub = n_.subscribe("/mapper/pathToUnknown", 1, &maze_navigator_node::pathToUnknownCallback, this);
obj_rec_ = n_.subscribe("/object_identifier/positions_in", 1, &maze_navigator_node::objRecCallback, this);
twist_publisher_ = n_.advertise<geometry_msgs::Twist>("/motor_controller/twist", 1000);
//Rohit: publish mode
//mode_publisher_ = n_.advertise<std_msgs::String>("/maze_navigator/mode", 1000);
mode_publisher_ = n_.advertise<std_msgs::Int16>("/maze_navigator/mode", 1000);
prev_mode_publisher_ = n_.advertise<std_msgs::Int16>("/maze_navigator/prevmode", 1000);
node_creation_publisher_ = n_.advertise<robo_cartesian_controllers::Node>("/node_creation", 100);
//service client
client = n_.serviceClient<mapper::WallInFront>("/wall_in_front");
updateCounter = 0;
}
void initialize(UAS &uas_,
ros::NodeHandle &nh,
diagnostic_updater::Updater &diag_updater)
{
bool mode_tx;
uas = &uas_;
nh.param("optical_flow_tx", mode_tx, false);
if (!mode_tx)
flow_pub = nh.advertise<mavros_extras::OpticalFlow>("optical_flow", 10);
else
flow_sub = nh.subscribe("optical_flow", 10, &PX4FlowPlugin::send_flow_cb, 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;
}
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////Constructeur////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
A11::A11(ros::NodeHandle nh)
{
cout<<"Initialisation du callback"<<endl;
//VREPsubSensor = nh.subscribe("vrep/Sensor20", 1,&A11::SensorCallback,this);
VREPsubStopSensor = nh.subscribe("vrep/StopSensor", 1000, &A11::StopSensorCallback, this);
VREPsubRailSensor = nh.subscribe("vrep/RailSensor", 1000, &A11::RailSensorCallback, this);
VREPsubSwitchSensor = nh.subscribe("vrep/SwitchSensor", 1000, &A11::SwitchSensorCallback, this);
//Capteurs Capteurs(noeud);
ShStop = nh.advertise<std_msgs::Int32>("/commande/ArretNavette", 1);
ShStart = nh.advertise<std_msgs::Int32>("/commande/DemarrerNavette", 1);
AigDev = nh.advertise<std_msgs::Int32>("/commande/DeverouilleAiguillage", 1);
AigVer = nh.advertise<std_msgs::Int32>("/commande/VerouilleAiguillage", 1);
AigGauche = nh.advertise<std_msgs::Int32>("/commande/AiguillageGauche", 1);
AigDroit = nh.advertise<std_msgs::Int32>("/commande/AiguillageDroit", 1);
M=2;
for (int i=0;i<taille_tab;i++) tab_AI[i]=0;//=0 pas de navette; =-1 navette à gauche; =1 navette à droite
tab_att=0;
tab_arr=0;
Nav_CPD=0;
Nav_CPG=0;
PS_past=0;
//this->A11Attente(0);
}
Explore(std::string name):
as_(nh_, name, false),
action_name_(name),
ac_("move_base", true)
{
as_.registerGoalCallback(boost::bind(&Explore::goalCB, this));
as_.registerPreemptCallback(boost::bind(&Explore::preemptCB, this));
as_.start();
firstGoalSend = false;
//all_balls = nh_.subscribe < geometry_msgs::PoseArray > ("allBalls", 10, &ChooseAccessibleBalls::allBallsCb, this);
alghoritm_state_sub_ = nh_.subscribe("alghoritm_state", 1, &Explore::alghoritmStateCallBack, this);
posSub = nh_.subscribe("/amcl_pose", 1, &Explore::poseCallback, this);
deadlock_service_state_sub = nh_.subscribe("/deadlock_service_state", 1, &Explore::deadlockServiceStateCb, this);
explore_state_ = STOP;
explore_ = false;
}
sensor_msgs::JointState getCurrentJointState(std::string& topicName, ros::NodeHandle& n)
{
received_js = false;
ros::Subscriber jsub = n.subscribe(topicName, 10, jointStateCallback);
ROS_INFO("Waiting until current joint state arrives...");
while (!received_js)
{
ros::spinOnce();
}
ROS_INFO("Joint state received.");
// ROS_INFO_STREAM(js);
return js;
}
void setup()
{
Wire.begin();
nh.initNode();
nh.subscribe(joint_1_set_setpoint_listener);
nh.advertise(joint_1_position_publisher);
nh.subscribe(joint_2_set_setpoint_listener);
nh.advertise(joint_2_position_publisher);
nh.subscribe(joint_3_set_setpoint_listener);
nh.advertise(joint_3_position_publisher);
joint_1_current_setpoint = 0.0; // Default setpoint
joint_1_new_setpoint = 0.0;
joint_2_current_setpoint = 0.0; // Default setpoint
joint_2_new_setpoint = 0.0;
joint_3_current_setpoint = 0.0; // Default setpoint
joint_3_new_setpoint = 0.0;
}
