int main(int argc, char** argv) {
ros::init(argc, argv, "batteryInfo");
nodeHandle = ros::NodeHandlePtr(new ros::NodeHandle("~"));
getNamePrefix();
ROS_INFO("Subscribing to /diagnostics ...");
ros::Subscriber diagnosticsSub = nodeHandle->subscribe("/diagnostics", 1, diagnosticsCallback);
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "pose_generator");
_handle = ros::NodeHandlePtr(new ros::NodeHandle(""));
_odom.header.frame_id = "map";
_x = _y = 0;
_theta = 0;
_correct_theta = false;
_correct_lateral = false;
_iteration_theta = 0; _iteration_lateral = 0;
_turn_accum = 0;
_heading = 0;
_see_front_plane = false;
_ringbuffer.reserve(_ringbuffer_max_size);
ros::Subscriber sub_enc = _handle->subscribe("/arduino/encoders",10,callback_encoders);
ros::Subscriber sub_turn_angle = _handle->subscribe("/controller/turn/angle",10,callback_turn_angle);
ros::Subscriber sub_turn_done = _handle->subscribe("/controller/turn/done",10,callback_turn_done);
ros::Subscriber sub_ir = _handle->subscribe("/perception/ir/distance",10,callback_ir);
ros::Subscriber sub_planes = _handle->subscribe("/vision/obstacles/planes",10,callback_planes);
ros::Subscriber sub_crash = _handle->subscribe("/perception/imu/peak", 10, callback_crash);
_pub_odom = _handle->advertise<nav_msgs::Odometry>("/pose/odometry/",10,(ros::SubscriberStatusCallback)connect_odometry_callback);
_pub_viz = _handle->advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
_pub_compass = _handle->advertise<std_msgs::Int8>("/pose/compass", 10, (ros::SubscriberStatusCallback)connect_compass_callback);
_srv_raycast = _handle->serviceClient<navigation_msgs::Raycast>("/mapping/raycast");
ros::spin();
return 0;
}
void subscribe() {
subscriber = nodeHandle->subscribe(subscriberTopic, subscriberQueueSize,
&callback, subscriberTransportHints);
}
int init(PhantomState *s)
{
if(!s)
{
ROS_FATAL("Internal error. PhantomState is NULL.");
return -1;
}
node_ = ros::NodeHandlePtr(new ros::NodeHandle);
pnode_ = ros::NodeHandlePtr(new ros::NodeHandle("~"));
pnode_->param(std::string("tf_prefix"), tf_prefix_, std::string(""));
// Vertical displacement from base_link to link_0. Defaults to Omni offset.
pnode_->param(std::string("table_offset"), table_offset_, .135);
// Force feedback damping coefficient
pnode_->param(std::string("damping_k"), damping_k_, 0.001);
// On startup device will generate forces to hold end-effector at origin.
pnode_->param(std::string("locked"), locked_, false);
// Check calibration status on start up and calibrate if necessary.
pnode_->param(std::string("calibrate"), calibrate_, false);
//Frame attached to the base of the phantom (NAME/base_link)
base_link_name_ = "base_link";
//Publish on NAME/pose
std::string pose_topic_name = "pose";
pose_publisher_ = node_->advertise<geometry_msgs::PoseStamped>(pose_topic_name, 100);
//Publish button state on NAME/button
std::string button_topic = "button";
button_publisher_ = node_->advertise<sensable_phantom::PhantomButtonEvent>(button_topic, 100);
//Subscribe to NAME/force_feedback
std::string force_feedback_topic = "force_feedback";
wrench_sub_ = node_->subscribe(force_feedback_topic, 100, &PhantomROS::wrench_callback, this);
//Frame of force feedback (NAME/sensable_origin)
sensable_frame_name_ = "sensable_origin";
for (int i = 0; i < 7; i++)
{
std::ostringstream stream1;
stream1 << "link_" << i;
link_names_[i] = std::string(stream1.str());
}
state_ = s;
state_->buttons[0] = 0;
state_->buttons[1] = 0;
state_->buttons_prev[0] = 0;
state_->buttons_prev[1] = 0;
hduVector3Dd zeros(0, 0, 0);
state_->velocity = zeros;
state_->inp_vel1 = zeros; //3x1 history of velocity
state_->inp_vel2 = zeros; //3x1 history of velocity
state_->inp_vel3 = zeros; //3x1 history of velocity
state_->out_vel1 = zeros; //3x1 history of velocity
state_->out_vel2 = zeros; //3x1 history of velocity
state_->out_vel3 = zeros; //3x1 history of velocity
state_->pos_hist1 = zeros; //3x1 history of position
state_->pos_hist2 = zeros; //3x1 history of position
state_->lock = locked_;
state_->lock_pos = zeros;
state_->hd_cur_transform = hduMatrix::createTranslation(0, 0, 0);
return 0;
}