void ARBundlePublisher::arInit ()
{
ROS_INFO("Starting arInit");
arInitCparam (&cam_param_);
ROS_INFO ("*** Camera Parameter ***");
arParamDisp (&cam_param_);
// load in the object data - trained markers and associated bitmap files
if ((object = ar_object::read_ObjData (pattern_filename_, &objectnum)) == NULL)
ROS_BREAK ();
ROS_INFO("Objectfile num = %d", objectnum);
// load in the transform data - transform of marker frame wrt center frame
if ((tfs= ar_transforms::read_Transforms (transforms_filename_, objectnum)) == NULL)
ROS_BREAK ();
ROS_INFO("Read in transforms successfully");
sz_ = cvSize (cam_param_.xsize, cam_param_.ysize);
#if ROS_VERSION_MINIMUM(1, 9, 0)
// FIXME: Why is this not in the object
cv_bridge::CvImagePtr capture_;
#else
// DEPRECATED: Fuerte support ends when Hydro is released
capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
#endif
}
StageNode::StageNode(int argc, char** argv, bool gui, const char* fname)
{
this->sim_time.fromSec(0.0);
this->base_last_cmd.fromSec(0.0);
double t;
ros::NodeHandle localn("~");
if(!localn.getParam("base_watchdog_timeout", t))
t = 0.2;
this->base_watchdog_timeout.fromSec(t);
// We'll check the existence of the world file, because libstage doesn't
// expose its failure to open it. Could go further with checks (e.g., is
// it readable by this user).
struct stat s;
if(stat(fname, &s) != 0)
{
ROS_FATAL("The world file %s does not exist.", fname);
ROS_BREAK();
}
// initialize libstage
Stg::Init( &argc, &argv );
if(gui)
this->world = new Stg::WorldGui(800, 700, "Stage (ROS)");
else
this->world = new Stg::World();
// Apparently an Update is needed before the Load to avoid crashes on
// startup on some systems.
// As of Stage 4.1.1, this update call causes a hang on start.
//this->UpdateWorld();
this->world->Load(fname);
// We add our callback here, after the Update, so avoid our callback
// being invoked before we're ready.
this->world->AddUpdateCallback((Stg::world_callback_t)s_update, this);
this->world->ForEachDescendant((Stg::model_callback_t)ghfunc, this);
if (lasermodels.size() != positionmodels.size())
{
ROS_FATAL("number of position models and laser models must be equal in "
"the world file.");
ROS_BREAK();
}
size_t numRobots = positionmodels.size();
ROS_INFO("found %u position/laser pair%s in the file",
(unsigned int)numRobots, (numRobots==1) ? "" : "s");
this->laserMsgs = new sensor_msgs::LaserScan[numRobots];
this->odomMsgs = new nav_msgs::Odometry[numRobots];
this->groundTruthMsgs = new nav_msgs::Odometry[numRobots];
this->colorMsgs = new my_stage::Color[numRobots];
}
int main(int argc, char ** argv)
{
ros::init(argc, argv, "save_utm_file");
ros::NodeHandle n;
std::string file_path = "utm_base_station.yaml";
ROS_INFO("Opening %s...", file_path.c_str());
FILE * yaml_file;
yaml_file = fopen(file_path.c_str() , "w");
if(yaml_file == NULL)
{
ROS_FATAL("Error opnening %s!", file_path.c_str());
ROS_BREAK();
}
ROS_INFO("Done.");
ROS_INFO("Subscribing to UTM topic...");
ros::Subscriber sub = n.subscribe("gps/utm", 10, callback);
ROS_INFO("Done.");
ros::Time start_time = ros::Time::now();
bool waiting_for_message = true;
ROS_INFO("Waiting for UTM message...");
ros::Rate r(5.0);
while(ros::ok() && waiting_for_message)
{
if(ros::Time::now() - utm_msg.header.stamp < ros::Duration(TIMEOUT))
{
fprintf(yaml_file, "base_position:\n x: %lf\n y: %lf\n z: %lf\n\n covariance: [", utm_msg.position.x, utm_msg.position.y, utm_msg.position.z);
for(int i=0 ; i<utm_msg.position_covariance.size() ; i++)
{
fprintf(yaml_file, "%lf%c", utm_msg.position_covariance[i], (i==utm_msg.position_covariance.size()-1 ? ']' : ','));
}
waiting_for_message = false;
}
if(ros::Time::now() - start_time > ros::Duration(TIMEOUT))
{
ROS_FATAL("Timeout waiting for UTM message. Are you sure the base station node is online?");
ROS_BREAK();
}
ros::spinOnce();
r.sleep();
}
fclose(yaml_file);
ROS_INFO("File saved. Goodbye!");
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "USB2_F_7001_node");
ros::NodeHandle n;
ros::NodeHandle pn("~");
rx_pub = n.advertise<can_msgs::CANFrame>("/can_bus_rx", 10);
ros::Subscriber tx_sub = n.subscribe<can_msgs::CANFrame>("/can_bus_tx", 10, CANFrameToSend);
std::string port;
//pn.param<std::string>("port", port, "/dev/ttyUSB0");
pn.param<std::string>("port", port, "/dev/serial/by-id/usb-LAWICEL_CANUSB_LWVPMVC4-if00-port0");
int bit_rate;
pn.param("bit_rate", bit_rate, 5);
can_usb_adapter = new USB2_F_7001(&port, &CANFrameReceived);
ROS_INFO("USB2_F_7001 -- Opening CAN bus...");
if( !can_usb_adapter->openCANBus((USB2_F_7001_BitRate)bit_rate) )
{
ROS_FATAL("USB2_F_7001 -- Failed to open the CAN bus!");
ROS_BREAK();
}
ROS_INFO("USB2_F_7001 -- The CAN bus is now open!");
ros::spin();
delete can_usb_adapter;
return(0);
}
void NavViewPanel::onToolClicked( wxCommandEvent& event )
{
switch( event.GetId() )
{
case ID_MOVE_TOOL:
{
delete current_tool_;
current_tool_ = new MoveTool( this );
}
break;
case ID_GOAL_TOOL:
{
delete current_tool_;
current_tool_ = new PoseTool( this, true );
}
break;
case ID_POSE_TOOL:
{
delete current_tool_;
current_tool_ = new PoseTool( this, false );
}
break;
default:
ROS_BREAK();
}
ROS_ASSERT( current_tool_ );
}
// Create a ROS publisher and suscriber for the tacta_belt
int main(int argc, char** argv)
{
ros::init(argc, argv, "tacta_wrist");
ros::NodeHandle n;
ros::Publisher state;
ros::Rate r(TACTA_BELT_STATE_UPDATE);
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = n.subscribe ("/feedback_devices/tacta_wrist/input", 1, wrist_cb);
// Change the next line according to your port name and baud rate
try{ device.open("/dev/ttyUSB0", 19200); }
catch(cereal::Exception& e)
{
ROS_FATAL("Failed to open the serial port!!!");
ROS_BREAK();
}
ROS_INFO("The serial port is opened.");
//On Init Enable B-Cast on All Channels
data[0] = TACTA_BELT_ADDRESS;
data[1] = TACTA_BELT_ENB_OUTPUT;
data[2] = TACTA_BELT_BCAST;
data[3] = TACTA_BELT_BCAST;
data[4] = data[0] ^ data[1] ^ data[2] ^ data[3];
device.write(data, 5);
ros::spin();
}
void SerialServer::readParameters()
{
std::string port_name, port_type;
LxSerial::PortType lx_port_type;
int baud_rate;
double watchdog_interval;
ROS_INFO("Reading parameters");
ROS_ASSERT(nh_.getParam("port_name", port_name));
ROS_ASSERT(nh_.getParam("port_type", port_type));
ROS_ASSERT(nh_.getParam("baud_rate", baud_rate));
nh_.param<double>("watchdog_interval", watchdog_interval, 10);
if (port_type == "RS232")
lx_port_type = LxSerial::RS232;
else if (port_type == "RS485_FTDI")
lx_port_type = LxSerial::RS485_FTDI;
else if (port_type == "RS485_EXAR")
lx_port_type = LxSerial::RS485_EXAR;
else if (port_type == "RS485_SMSC")
lx_port_type = LxSerial::RS485_SMSC;
else if (port_type == "TCP")
lx_port_type = LxSerial::TCP;
else
{
ROS_FATAL_STREAM("Unknown port type " << port_type);
ROS_BREAK();
return;
}
ROS_ASSERT(serial_port_.port_open(port_name, lx_port_type));
ROS_ASSERT(serial_port_.set_speed_int(baud_rate));
ROS_ASSERT(watchdog_.set(watchdog_interval));
}
const Path&
OccupancyMap::prepareShortestPaths(double x, double y,
double min_distance, double max_distance,
double max_occ_dist,
bool allow_unknown) {
endpoints_.clear();
if (map_ == NULL) {
ROS_WARN("OccupancyMap::prepareShortestPaths() Map not set");
return endpoints_;
}
if (map_->max_occ_dist < max_occ_dist) {
ROS_ERROR("OccupancyMap::prepareShortestPaths() CSpace has been calculated "
"up to %f, but max_occ_dist=%.2f",
map_->max_occ_dist, max_occ_dist);
ROS_BREAK();
}
initializeSearch(x, y);
Node curr_node;
while (nextNode(max_occ_dist, &curr_node, allow_unknown)) {
double node_dist = curr_node.true_cost * map_->scale;
if (min_distance <= node_dist && node_dist < max_distance) {
float x = MAP_WXGX(map_, curr_node.coord.first);
float y = MAP_WYGY(map_, curr_node.coord.second);
endpoints_.push_back(Eigen::Vector2f(x, y));
} else if (node_dist > max_distance) {
break;
}
}
return endpoints_;
}
int
main (int argc, char **argv)
{
ros::init (argc, argv, "pgr_camera");
typedef dynamic_reconfigure::Server < pgr_camera_driver::PGRCameraConfig > Server;
Server server;
try
{
boost::shared_ptr < PGRCameraNode > pn (new PGRCameraNode (ros::NodeHandle(),ros::NodeHandle("~")));
Server::CallbackType f = boost::bind (&PGRCameraNode::configure, pn, _1, _2);
server.setCallback (f);
ros::spin ();
} catch (std::runtime_error & e)
{
ROS_FATAL ("Uncaught exception: '%s', aborting.", e.what ());
ROS_BREAK ();
}
return 0;
}
aiReturn Seek( size_t offset, aiOrigin origin)
{
uint8_t* new_pos = 0;
switch (origin)
{
case aiOrigin_SET:
new_pos = res_.data.get() + offset;
break;
case aiOrigin_CUR:
new_pos = pos_ + offset; // TODO is this right? can offset really not be negative
break;
case aiOrigin_END:
new_pos = res_.data.get() + res_.size - offset; // TODO is this right?
break;
default:
ROS_BREAK();
}
if (new_pos < res_.data.get() || new_pos > res_.data.get() + res_.size)
{
return aiReturn_FAILURE;
}
pos_ = new_pos;
return aiReturn_SUCCESS;
}
void AudioProcessor::checkForErrors(FMOD_RESULT result)
{
if (result != FMOD_OK)
{
ROS_ERROR("FMOD error! (%d) %s", result, FMOD_ErrorString(result));
ROS_BREAK();
}
}
void spin()
{
initRCservo();
rcservo_EnterPlayMode();
com4_ics_init();
ServoLibrary::iterator i;
for(i = servos_.begin(); i != servos_.end(); ++i)
{
//std::string& joint_name = i->first;
Servo& servo = i->second;
if (servo.bus_ == Servo::COM4)
{
usleep(1);
//com4_ics_pos(servo.channel_, 8193);
// TODO: there is a command to tell the servo to lock into its current position, but
// doesnt seem to work.
}
}
running_ = true;
static pthread_t controlThread_;
static pthread_attr_t controlThreadAttr_;
int rv;
if (playaction_thread_)
{
if ((rv = pthread_create(&controlThread_, &controlThreadAttr_, playActionThread, this)) != 0)
{
ROS_FATAL("Unable to create control thread: rv = %d", rv);
ROS_BREAK();
}
}
ros::spin();
running_ = false;
if (playaction_thread_)
pthread_join(controlThread_, NULL);//(void **)&rv);
rcservo_Close();
for(i = servos_.begin(); i != servos_.end(); ++i)
{
//std::string& joint_name = i->first;
Servo& servo = i->second;
if (servo.bus_ == Servo::COM4)
{
com4_ics_set_stretch(servo.channel_, 4);
usleep(1);
com4_ics_set_speed(servo.channel_, 64);
usleep(1);
com4_ics_pos(servo.channel_, 0);
usleep(1);
}
}
com4_ics_close();
}
int main(int argc, char ** argv)
{
ros::init(argc, argv, "rtk_base_station");
ROS_INFO("RTKlib for ROS Base Station Edition");
ros::NodeHandle n;
ros::NodeHandle pn("~");
std::string port;
pn.param<std::string>("port", port, "/dev/ttyACM1");
int baudrate;
pn.param("baudrate", baudrate, 115200);
cereal::CerealPort serial_gps;
try{ serial_gps.open(port.c_str(), baudrate); }
catch(cereal::Exception& e)
{
ROS_FATAL("RTK -- Failed to open the serial port!!!");
ROS_BREAK();
}
char buffer[350];
int count;
buffer[108]=0;
buffer[0]='l';
buffer[1]='s';
buffer[2]='e';
buffer[3]=1;
ros::Publisher pub = n.advertise<std_msgs::ByteMultiArray>("base_station/gps/raw_data", 100);
ROS_INFO("RTK -- Streaming data...");
while(ros::ok())
{
try{ count = serial_gps.read(buffer, 300, 1000); }
catch(cereal::TimeoutException& e)
{
ROS_WARN("RTK -- Failed to get data from GPS!");
}
std_msgs::ByteMultiArray raw_msg;
for(int i=0 ; i<count ; i++)
{
raw_msg.data.push_back(buffer[i]);
}
pub.publish(raw_msg);
ros::Duration(0.1).sleep();
}
return 0;
}
void setRetryTimeout(ros::WallDuration timeout)
{
if (timeout < ros::WallDuration(0))
{
ROS_FATAL("retry timeout must not be negative.");
ROS_BREAK();
}
g_retry_timeout = timeout;
}
PollSet::PollSet()
: sockets_changed_(false)
{
if ( create_signal_pair(signal_pipe_) != 0 ) {
ROS_FATAL("create_signal_pair() failed");
ROS_BREAK();
}
addSocket(signal_pipe_[0], boost::bind(&PollSet::onLocalPipeEvents, this, _1));
addEvents(signal_pipe_[0], POLLIN);
}
void init(const M_string& remappings)
{
M_string::const_iterator it = remappings.find("__master");
if (it != remappings.end())
{
g_uri = it->second;
}
if (g_uri.empty())
{
char *master_uri_env = NULL;
#ifdef _MSC_VER
_dupenv_s(&master_uri_env, NULL, "ROS_MASTER_URI");
#else
master_uri_env = getenv("ROS_MASTER_URI");
#endif
if (!master_uri_env)
{
ROS_FATAL( "ROS_MASTER_URI is not defined in the environment. Either " \
"type the following or (preferrably) add this to your " \
"~/.bashrc file in order set up your " \
"local machine as a ROS master:\n\n" \
"export ROS_MASTER_URI=http://localhost:11311\n\n" \
"then, type 'roscore' in another shell to actually launch " \
"the master program.");
ROS_BREAK();
}
g_uri = master_uri_env;
#ifdef _MSC_VER
// http://msdn.microsoft.com/en-us/library/ms175774(v=vs.80).aspx
free(master_uri_env);
#endif
}
// Split URI into
if (!network::splitURI(g_uri, g_host, g_port))
{
ROS_FATAL( "Couldn't parse the master URI [%s] into a host:port pair.", g_uri.c_str());
ROS_BREAK();
}
}
Path OccupancyMap::astar(double startx, double starty,
double stopx, double stopy,
double max_occ_dist /* = 0.0 */,
bool allow_unknown /* = false */) {
Path path;
if (map_ == NULL) {
ROS_WARN("OccupancyMap::astar() Map not set");
return path;
}
int stopi = MAP_GXWX(map_, stopx), stopj = MAP_GYWY(map_, stopy);
if (!MAP_VALID(map_, stopi ,stopj)) {
ROS_ERROR("OccupancyMap::astar() Invalid stopping position");
ROS_BREAK();
}
if (map_->max_occ_dist < max_occ_dist) {
ROS_ERROR("OccupancyMap::astar() CSpace has been calculated up to %f, "
"but max_occ_dist=%.2f",
map_->max_occ_dist, max_occ_dist);
ROS_BREAK();
}
initializeSearch(startx, starty);
// Set stop to use heuristic
stopi_ = stopi;
stopj_ = stopj;
bool found = false;
Node curr_node;
while (nextNode(max_occ_dist, &curr_node, allow_unknown)) {
if (curr_node.coord.first == stopi && curr_node.coord.second == stopj) {
found = true;
break;
}
}
// Recreate path
if (found) {
buildPath(stopi, stopj, &path);
}
return Path(path.rbegin(), path.rend());
}
/** Get video mode.
*
* @param camera points to DC1394 camera struct
* @param[in,out] video_mode Config parameter for this option,
* updated if the camera does not support the
* requested value
* @return corresponding dc1394video_mode_t enum value selected
*/
dc1394video_mode_t getVideoMode(dc1394camera_t *camera,
std::string &video_mode)
{
for (int vm = DC1394_VIDEO_MODE_MIN;
vm <= DC1394_VIDEO_MODE_MAX;
++vm)
{
if (video_mode_names_[vm-DC1394_VIDEO_MODE_MIN] == video_mode)
{
// found the requested mode
dc1394video_modes_t vmodes;
dc1394error_t err =
dc1394_video_get_supported_modes(camera, &vmodes);
if (err != DC1394_SUCCESS)
{
ROS_FATAL("unable to get supported video modes");
// TODO raise exception
return (dc1394video_mode_t) 0;
}
// see if requested mode is available
for (uint32_t i = 0; i < vmodes.num; ++i)
{
if (vmodes.modes[i] == vm)
return (dc1394video_mode_t) vm; // yes: success
}
// requested mode not available, revert to current mode of camera
ROS_ERROR_STREAM("Video mode " << video_mode
<< " not supported by this camera");
dc1394video_mode_t current_mode;
err = dc1394_video_get_mode(camera, ¤t_mode);
if (err != DC1394_SUCCESS)
{
ROS_FATAL("unable to get current video mode");
// TODO raise exception
return (dc1394video_mode_t) 0;
}
// TODO list available modes
// change video_mode parameter to show current mode of camera
video_mode = videoModeName(current_mode);
return current_mode;
}
}
// request video mode does not match any known string
ROS_FATAL_STREAM("Unknown video_mode:" << video_mode);
ROS_BREAK();
// TODO raise exception
//CAM_EXCEPT(camera1394::Exception, "Unsupported video_mode");
return (dc1394video_mode_t) 0;
}
CANUSB::CANUSB(std::string * serial_port_name, boost::function<void(std::string*)> f) : serial_port_()
{
open_ = false;
try{ serial_port_.open((char*)serial_port_name->c_str(), 115200); }
catch(cereal::Exception& e)
{
ROS_FATAL("CANUSB - %s - Failed to open the serial port!!!", __FUNCTION__);
ROS_BREAK();
}
receivedFrameCallback = f;
}
void OccupancyMap::initializeSearch(double startx, double starty) {
starti_ = MAP_GXWX(map_, startx);
startj_ = MAP_GYWY(map_, starty);
if (!MAP_VALID(map_, starti_, startj_)) {
ROS_ERROR("OccupancyMap::initializeSearch() Invalid starting position");
ROS_BREAK();
}
int ncells = map_->size_x * map_->size_y;
if (ncells_ != ncells) {
ncells_ = ncells;
costs_.reset(new float[ncells]);
prev_i_.reset(new int[ncells]);
prev_j_.reset(new int[ncells]);
}
// TODO: Return to more efficient lazy-initialization
// // Map is large and initializing costs_ takes a while. To speedup,
// // partially initialize costs_ in a rectangle surrounding start and stop
// // positions + margin. If you run up against boundary, initialize the rest.
// int margin = 120;
// init_ul_ = make_pair(max(0, min(starti_, stopi) - margin),
// max(0, min(startj_, stopj) - margin));
// init_lr_ = make_pair(min(map_->size_x, max(starti_, stopi) + margin),
// min(map_->size_y, max(startj_, stopj) + margin));
// for (int j = init_ul.second; j < init_lr.second; ++j) {
// for (int i = init_ul.first; i < init_lr.first; ++i) {
// int ind = MAP_INDEX(map_, i, j);
// costs_[ind] = std::numeric_limits<float>::infinity();
// }
// }
// full_init_ = false;
for (int i = 0; i < ncells_; ++i) {
costs_[i] = std::numeric_limits<float>::infinity();
prev_i_[i] = -1;
prev_j_[i] = -1;
}
int start_ind = MAP_INDEX(map_, starti_, startj_);
costs_[start_ind] = 0.0;
prev_i_[starti_] = starti_;
prev_j_[startj_] = startj_;
Q_.reset(new set<Node, NodeCompare>());
Q_->insert(Node(make_pair(starti_, startj_), 0.0, 0.0));
stopi_ = -1;
stopj_ = -1;
}
// Create a ROS publisher and suscriber for the tacta_belt
int main(int argc, char** argv)
{
ros::init(argc, argv, "tacta_fingers");
ros::NodeHandle n;
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = n.subscribe ("/feedback_devices/tacta_grippers/input", 1, grippers_cb);
// Change the next line according to your port name and baud rate
try{ device.open("/dev/ttyUSB0", 19200); }
catch(cereal::Exception& e)
{
ROS_FATAL("Failed to open the serial port!!!");
ROS_BREAK();
}
ROS_INFO("The serial port has been opened.");
//On Init Enable B-Cast on All Channels
data_amp[0] = TACTA_BELT_ADDRESS;
data_amp[1] = TACTA_BELT_ENB_OUTPUT;
data_amp[2] = TACTA_BELT_BCAST;
data_amp[3] = TACTA_BELT_BCAST;
data_amp[4] = data_amp[0] ^ data_amp[1] ^ data_amp[2] ^ data_amp[3];
device.write(data_amp, 5);
ros::Rate r(100);
for (int i = 0; i < 100; i++){
//Right Hand output struct
rh_rf_output[i] = 0;
rh_lf_output[i] = 0;
//Left Hand output struct
lh_rf_output[i] = 0;
lh_lf_output[i] = 0;
}
while(ros::ok()){
tacta_output();
ros::spinOnce();
r.sleep();
}
ros::spin();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "windsonic_node");
ros::NodeHandle n;
ros::NodeHandle pn("~");
wind_pub = n.advertise<lse_sensor_msgs::Anemometer>("/wind", 10);
std::string port;
int baudrate;
pn.param<std::string>("port", port, "/dev/ttyUSB0");
pn.param("baudrate", baudrate, 38400);
pn.param<std::string>("frame_id", frame_id, "/base_anemometer");
ROS_INFO("%s", frame_id.c_str());
cereal::CerealPort serial_port;
try{ serial_port.open((char*)port.c_str(), baudrate); }
catch(cereal::Exception& e)
{
ROS_FATAL("WindSonic -- Failed to open serial port!");
ROS_BREAK();
}
ROS_INFO("WindoSonic -- Successfully connected to the WindSonic!");
if( !serial_port.startReadBetweenStream(boost::bind(&newDataCallback, _1), 0x02, 0x03) )
{
ROS_FATAL("WindSonic -- Failed to start streaming data!");
ROS_BREAK();
}
ROS_INFO("Windsonic -- Starting to stream data...");
ros::spin();
return(0);
}
/*
* Setup artoolkit
*/
void ARPublisher::arInit ()
{
arInitCparam (&cam_param_);
ROS_INFO ("*** Camera Parameter ***");
arParamDisp (&cam_param_);
// load in the object data - trained markers and associated bitmap files
if ((object = ar_object::read_ObjData (pattern_filename_, data_directory_, &objectnum)) == NULL)
ROS_BREAK ();
ROS_DEBUG ("Objectfile num = %d", objectnum);
sz_ = cvSize (cam_param_.xsize, cam_param_.ysize);
capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
configured_ = true;
}
void DriverNodelet::onInit() {
ROS_INFO("rslidar driver nodelet init");
// start the driver
RslidarDriver *driver =
RslidarDriverFactory::create_driver(getPrivateNodeHandle());
if (driver == nullptr) {
ROS_BREAK();
}
dvr_.reset(driver);
dvr_->init(getNodeHandle());
// spawn device poll thread
runing_ = true;
device_thread_ = boost::shared_ptr<boost::thread>(
new boost::thread(boost::bind(&DriverNodelet::device_poll, this)));
}
void ConnectionManager::start()
{
poll_manager_ = PollManager::instance();
poll_conn_ = poll_manager_->addPollThreadListener(boost::bind(&ConnectionManager::removeDroppedConnections,
this));
// Bring up the TCP listener socket
tcpserver_transport_ = TransportTCPPtr(new TransportTCP(&poll_manager_->getPollSet()));
if (!tcpserver_transport_->listen(network::getTCPROSPort(),
MAX_TCPROS_CONN_QUEUE,
boost::bind(&ConnectionManager::tcprosAcceptConnection, this, _1)))
{
ROS_FATAL("Listen on port [%d] failed", network::getTCPROSPort());
ROS_BREAK();
}
// Bring up the UDP listener socket
udpserver_transport_ = TransportUDPPtr(new TransportUDP(&poll_manager_->getPollSet()));
if (!udpserver_transport_->createIncoming(0, true))
{
ROS_FATAL("Listen failed");
ROS_BREAK();
}
}
void SerialInterface::output (unsigned char *output, int len)
{
int i;
ROS_DEBUG ("SerialInterface::output()");
serialport_bytes_tx_ += len;
//ROS_INFO ("Writing %d element(s): %s", len, output);
//ROS_DEBUG ("dev: %zd", (size_t) dev_);
//ROS_DEBUG ("FOO");
i = write (dev_, output, len);
if (i != len)
{
ROS_ERROR ("Error wrote %d out of %d element(s): %s", i, len, strerror (errno));
ROS_BREAK ();
}
}
// Create a ROS publisher and suscriber for the tacta_belt
int main(int argc, char** argv)
{
ros::init(argc, argv, "tacta_belt");
ros::NodeHandle n;
ros::Publisher state;
ros::Rate r(TACTA_BELT_STATE_UPDATE);
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = n.subscribe ("/feedback_devices/tacta_belt/input", 1, belt_cb);
//Advertise the two publishers, one for the commands and one for the gui
state = n.advertise<feedback_devices::tacta_belt>("/feedback_devices/tacta_belt/state", 1);
// Change the next line according to your port name and baud rate
try{ device.open("/dev/ttyUSB0", 19200); }
catch(cereal::Exception& e)
{
ROS_FATAL("Failed to open the serial port!!!");
ROS_BREAK();
}
ROS_INFO("The serial port is opened.");
//On Init Enable B-Cast on All Channels
data[0] = TACTA_BELT_ADDRESS;
data[1] = TACTA_BELT_ENB_OUTPUT;
data[2] = TACTA_BELT_BCAST;
data[3] = TACTA_BELT_BCAST;
data[4] = data[0] ^ data[1] ^ data[2] ^ data[3];
device.write(data, 5);
ros::spin();
// while(ros::ok())
// {
//
// // Get the reply, the last value is the timeout in ms
// try{ device.read(reply, REPLY_SIZE, TIMEOUT); }
// catch(cereal::TimeoutException& e)
// {
// ROS_ERROR("Timeout!");
// }
// ROS_INFO("Got this reply: %s", reply);
//
// ros::spinOnce();
// r.sleep();
// }
}
void GetGraphInfo (vertex *vertex_web, uint dimension, const char* graph_file){
FILE *file;
file = fopen (graph_file,"r");
if(file == NULL){
ROS_INFO("Can not open filename %s", graph_file);
ROS_BREAK();
}else{
ROS_INFO("Graph File Opened. Getting Graph Info.\n");
uint i,j;
float temp;
int r;
//Start Reading the File from FIRST_VID On:
for (i=0; i<FIRST_VID-1; i++){
r=fscanf (file, "%f", &temp);
}
for (i=0;i<dimension;i++){
r=fscanf (file, "%u", &vertex_web[i].id);
r=fscanf (file, "%f", &vertex_web[i].x);
vertex_web[i].x *= RESOLUTION; //convert to m
r=fscanf (file, "%f", &vertex_web[i].y);
vertex_web[i].y *= RESOLUTION; //convert to m
r=fscanf (file, "%u", &vertex_web[i].num_neigh);
for (j=0;j<vertex_web[i].num_neigh; j++){
r=fscanf (file, "%u", &vertex_web[i].id_neigh[j]);
r=fscanf (file, "%s", &vertex_web[i].dir[j]);
r=fscanf (file, "%u", &vertex_web[i].cost[j]); //can eventually be converted to meters also...
}
}
}
//printf ("[v=10], x = %f (meters)\n",vertex_web[10].x);
fclose(file);
}
void RosoutPanel::validateOrderedMessages()
{
#if VALIDATE_FILTERING
typedef std::set<uint32_t> S_u32;
S_u32 s;
V_u32::iterator it = ordered_messages_.begin();
V_u32::iterator end = ordered_messages_.end();
for (; it != end; ++it)
{
uint32_t id = *it;
if (!s.insert(id).second)
{
ROS_BREAK();
}
}
#endif
}
void ShapeMarker::onNewMessage( const MarkerConstPtr& old_message,
const MarkerConstPtr& new_message )
{
if (!shape_ || old_message->type != new_message->type)
{
delete shape_;
shape_ = 0;
Shape::Type shape_type = Shape::Cube;
switch( new_message->type )
{
case visualization_msgs::Marker::CUBE: shape_type = Shape::Cube; break;
case visualization_msgs::Marker::CYLINDER: shape_type = Shape::Cylinder; break;
case visualization_msgs::Marker::SPHERE: shape_type = Shape::Sphere; break;
default:
ROS_BREAK();
break;
}
shape_ = new Shape( shape_type, context_->getSceneManager(), scene_node_ );
handler_.reset( new MarkerSelectionHandler( this, MarkerID( new_message->ns, new_message->id ), context_ ));
handler_->addTrackedObjects( shape_->getRootNode() );
}
Ogre::Vector3 pos, scale, scale_correct;
Ogre::Quaternion orient;
transform(new_message, pos, orient, scale);
if (owner_ && (new_message->scale.x * new_message->scale.y
* new_message->scale.z == 0.0f))
{
owner_->setMarkerStatus(getID(), StatusProperty::Warn,
"Scale of 0 in one of x/y/z");
}
setPosition(pos);
setOrientation( orient * Ogre::Quaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ) );
scale_correct = Ogre::Quaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ) * scale;
shape_->setScale(scale_correct);
shape_->setColor(new_message->color.r, new_message->color.g,
new_message->color.b, new_message->color.a);
}