void GroupCommandController::commandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
{
#if TRACE_GroupCommandController_ACTIVATED
ROS_INFO("GroupCommandController: Start commandCB of robot %s!",robot_namespace_.c_str());
#endif
if(msg->data.size()!=joint_handles_.size())
{
ROS_ERROR_STREAM("GroupCommandController: Dimension (of robot " << robot_namespace_.c_str() << ") of command (" << msg->data.size() << ") does not match number of joints (" << joint_handles_.size() << ")! Not executing!");
cmd_flag_ = 0;
return;
}
// clear buffers for initial values
commands_buffer_.clear();
goal_buffer_.clear();
goal_factor_.clear();
for (size_t i=0; i<joint_handles_.size(); ++i)
{
commands_buffer_.push_back(msg->data[i]);
// set the factor of increment/decrement depending of the current joint value and the desired one
if (joint_handles_[i].getPosition() > msg->data[i])
goal_factor_.push_back(-1);
else
goal_factor_.push_back(1);
}
cmd_flag_ = 1; // set this flag to 1 to run the update method
#if TRACE_GroupCommandController_ACTIVATED
ROS_INFO("GroupCommandController: Finish commandCB of robot %s !",robot_namespace_.c_str());
ROS_INFO("GroupCommandController: of robot %s -> j0=%f, j1=%f, j2=%f, j3=%f, j4=%f, j5=%f, j6=%f",robot_namespace_.c_str(),commands_buffer_[0],commands_buffer_[1],commands_buffer_[2],commands_buffer_[3],commands_buffer_[4],commands_buffer_[5],commands_buffer_[6]);
#endif
}
void
KalmanDetectionFilter::drawFilterStates()
{
if(!current_filter_)
return;
geometry_msgs::PointStamped temp_msg, temp_msg_base_link;
temp_msg.header.frame_id = "odom";
temp_msg.header.stamp = ros::Time(0);
cv::Mat img = cv::Mat::zeros(500, 500, CV_8UC3);
float px_per_meter = 50.0;
float offset = 250;
temp_msg.point.x = current_filter_->statePost.at<float>(0);
temp_msg.point.y = current_filter_->statePost.at<float>(1);
temp_msg.point.z = 0.0;
if( listener_.canTransform( "base_link", temp_msg.header.frame_id, temp_msg.header.stamp))
{
listener_.transformPoint("base_link",temp_msg,temp_msg_base_link);
}
else
{
ROS_ERROR_STREAM("cannot transform filter from odom to base_link");
return;
}
cv::Point mean(temp_msg_base_link.point.x * px_per_meter,
temp_msg_base_link.point.y * px_per_meter);
float rad_x = current_filter_->errorCovPost.at<float>(0,0) * px_per_meter;
float rad_y = current_filter_->errorCovPost.at<float>(1,1) * px_per_meter;
cv::circle(img, mean+cv::Point(0,offset), 5, cv::Scalar(255,0,0));
cv::ellipse(img, mean+cv::Point(0,offset), cv::Size(rad_x, rad_y), 0, 0, 360, cv::Scalar(0,255,0));
//printFilterState();
std_msgs::Header header;
sensor_msgs::ImagePtr debug_img_msg = cv_bridge::CvImage(header,"rgb8",img).toImageMsg();
pub_debug_img.publish(debug_img_msg);
}
bool MCU::rcvMCUMsg(double & angle)
{
bool result = false;
char b0, b1, b2, b3, b4, b5;
if (!serialPort.get(b0)) return false;
if (b0 != MCU_RESP_START_BYTE) return false;
if (!serialPort.get(b1)) return false;
if (!serialPort.get(b2)) return false;
if (!serialPort.get(b3)) return false;
if (!serialPort.get(b4)) return false;
if (!serialPort.get(b5)) return false;
// Perform checksum
char checksum;
checksum = b0 ^ b1 ^ b2 ^ b3 ^ b4;
if (checksum == b5)
{
int stepPosition = (b1 << 24) + (b2 << 16) + (b3 << 8) + (b4 & 0xFF);
angle = static_cast<double>(stepPosition) / STEPS_PER_REV * 360;
// ROS_INFO_STREAM("MCU::rcvMCUMsg: Received current angle: " << currAngle << ", stepPosition: " << stepPosition << "\n"
// << "b0 = " << std::hex << "0x" << static_cast<int>(b0) << "\n"
// << "b1 = " << std::hex << "0x" << static_cast<int>(b1) << "\n"
// << "b2 = " << std::hex << "0x" << static_cast<int>(b2) << "\n"
// << "b3 = " << std::hex << "0x" << static_cast<int>(b3) << "\n"
// << "b4 = " << std::hex << "0x" << static_cast<int>(b4) << "\n"
// << "b5 = " << std::hex << "0x" << static_cast<int>(b5));
result = true;
}
else
{
ROS_ERROR_STREAM("MCU::rcvMCUMsg: Bad checksum of 0x" << std::hex << checksum << ", expected 0x" << b5);
result = false;
}
return result;
}
bool ROSRuntimeUtils::store_tf_broadcasters(std::string &package_path, std::string &file_name)
{
std::string filepath = package_path+file_name;
std::ofstream output_file(filepath.c_str(), std::ios::out);// | std::ios::app);
if (output_file.is_open())
{
ROS_INFO_STREAM("Storing results in: "<<filepath);
}
else
{
ROS_ERROR_STREAM("Unable to open file");
return false;
}
output_file << "<launch>";
//have calibrated transforms
double roll, pitch, yaw, x_position, y_position, z_position;
tf::Vector3 origin;
tf::Matrix3x3 orientation;
for (int i=0; i<calibrated_transforms_.size(); i++)
{
origin = calibrated_transforms_.at(i).getOrigin();
x_position=origin.getX();
y_position=origin.getY();
z_position=origin.getZ();
orientation = calibrated_transforms_.at(i).getBasis();
orientation.getEulerYPR(yaw, pitch, roll);
output_file<<"\n";
output_file<<" <node pkg=\"tf\" type=\"static_transform_publisher\" name=\"world_to_camera"<<i<<"\" args=\"";
//tranform publisher launch files requires x y z yaw pitch roll
output_file<<x_position<< ' '<<y_position<< ' '<<z_position<< ' '<<yaw<< ' '<<pitch<< ' '<<roll ;
output_file<<" "<<world_frame_;
output_file<<" "<<camera_intermediate_frame_[i];
output_file<<" 100\" />";
}
output_file<<"\n";
output_file << "</launch>";
return true;
}
std::vector<urdf_traverser::JointPtr>
urdf_traverser::getChain(const LinkConstPtr& from_link, const LinkConstPtr& to_link)
{
assert(from_link);
assert(to_link);
std::vector<JointPtr> chain;
if (to_link->name == from_link->name) return chain;
LinkConstPtr curr = to_link;
LinkConstPtr pl = to_link->getParent();
while (curr && (curr->name != from_link->name))
{
// ROS_INFO_STREAM("Chain: "<<curr->name);
JointPtr pj = curr->parent_joint;
if (!pj)
{
ROS_ERROR("UrdfTraverser: End of chain at link '%s'", curr->name.c_str());
return std::vector<JointPtr>();
}
chain.push_back(pj);
curr = pl;
pl = curr->getParent();
// if (pl) ROS_INFO_STREAM("Parent: "<<pl->name);
}
if (curr->name != from_link->name)
{
ROS_ERROR_STREAM("UrdfTraverser: could not find link " <<
from_link->name << " while traversing up the chain starting from " <<
to_link->name << ". Failed to find parent chain!");
return std::vector<JointPtr>();
}
std::reverse(chain.begin(), chain.end());
return chain;
}
enum uvc_frame_format CameraDriver::GetVideoMode(std::string vmode){
if(vmode == "uncompressed") {
return UVC_COLOR_FORMAT_UNCOMPRESSED;
} else if (vmode == "compressed") {
return UVC_COLOR_FORMAT_COMPRESSED;
} else if (vmode == "yuyv") {
return UVC_COLOR_FORMAT_YUYV;
} else if (vmode == "uyvy") {
return UVC_COLOR_FORMAT_UYVY;
} else if (vmode == "rgb") {
return UVC_COLOR_FORMAT_RGB;
} else if (vmode == "bgr") {
return UVC_COLOR_FORMAT_BGR;
} else if (vmode == "mjpeg") {
return UVC_COLOR_FORMAT_MJPEG;
} else if (vmode == "gray8") {
return UVC_COLOR_FORMAT_GRAY8;
} else {
ROS_ERROR_STREAM("Invalid Video Mode: " << vmode);
ROS_WARN_STREAM("Continue using video mode: uncompressed");
return UVC_COLOR_FORMAT_UNCOMPRESSED;
}
};
void DisableLinkPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
model_ = _parent;
world_ = model_->GetWorld();
// Check for link element
if (!_sdf->HasElement("link")) {
ROS_ERROR("No link element present. DisableLinkPlugin could not be loaded.");
return;
}
link_name_ = _sdf->GetElement("link")->Get<std::string>();
// Get pointers to joints
link_ = model_->GetLink(link_name_);
if (link_) {
link_->SetEnabled(false);
// Output some confirmation
ROS_INFO_STREAM("DisableLinkPlugin loaded! Link: \"" << link_name_);
}
else
ROS_ERROR_STREAM("Link" << link_name_ << " not found! DisableLinkPlugin could not be loaded.");
}
CameraDriver( int camera_index = DEFAULT_CAMERA_INDEX )
:
nh( "~" ),
it( nh ),
camera( camera_index )
{
nh.param<int>( "camera_index", camera_index, DEFAULT_CAMERA_INDEX );
if ( not camera.isOpened() )
{
ROS_ERROR_STREAM( "Failed to open camera device!" );
ros::shutdown();
}
nh.param<int>( "fps", fps, DEFAULT_FPS );
ros::Duration period = ros::Duration( 1. / fps );
pub_image_raw = it.advertise( "image_raw", 1 );
frame = boost::make_shared< cv_bridge::CvImage >();
frame->encoding = sensor_msgs::image_encodings::BGR8;
timer = nh.createTimer( period, &CameraDriver::capture, this );
}
boost::shared_ptr<pcl17::PolygonMesh> SceneCloudView::convertToMesh(float* triangles, unsigned int maxverts)
{
if (maxverts == 0)
return boost::shared_ptr<pcl17::PolygonMesh>();
pcl17::PointCloud<pcl17::PointXYZ> cloud;
cloud.width = (int)(maxverts);
cloud.height = 1;
cloud.points.clear();
for (uint i = 0; i < 3 * maxverts; i += 3)
cloud.points.push_back(pcl17::PointXYZ(triangles[i], triangles[i + 1], triangles[i + 2]));
boost::shared_ptr<pcl17::PolygonMesh> mesh_ptr(new pcl17::PolygonMesh());
try
{
pcl17::toROSMsg(cloud, mesh_ptr->cloud);
}
catch (std::runtime_error e)
{
ROS_ERROR_STREAM("Error in converting cloud to image message: "
<< e.what());
}
mesh_ptr->polygons.resize(maxverts / 3);
for (size_t i = 0; i < mesh_ptr->polygons.size(); ++i)
{
pcl17::Vertices v;
v.vertices.push_back(i * 3 + 0);
v.vertices.push_back(i * 3 + 2);
v.vertices.push_back(i * 3 + 1);
mesh_ptr->polygons[i] = v;
}
return mesh_ptr;
}
std::string readLine(boost::asio::serial_port &port) {
std::string line = "";
bool inLine = false;
while (true) {
char in;
try {
boost::asio::read(port, boost::asio::buffer(&in, 1));
} catch (
boost::exception_detail::clone_impl <boost::exception_detail::error_info_injector<boost::system::system_error>> &err) {
ROS_ERROR("Error reading serial port.");
ROS_ERROR_STREAM(err.what());
return line;
}
if (!inLine && in == '$')
inLine = true;
if(inLine) {
if (in == '\n')
return line;
if (in == '\r')
return line;
line += in;
}
}
}
void DataCollector::onMouse( int event, int x, int y, int, void* ptr) {
if( event != cv::EVENT_LBUTTONDOWN )
return;
try {
DataCollector* that = (DataCollector*) ptr;
std::string image_path1 = makePngName(std::to_string(that->label), that->sub_dir);
std::string image_path2 = makeJp2Name(std::to_string(that->label), that->sub_dir);
if( imwrite(image_path1, that->rgb_ptr->image) && imwrite(image_path2, that->depth_ptr->image)) {
ROS_INFO_STREAM("Write to:" << image_path1<<"\n"<<image_path2);
that->label++;
} else {
ROS_ERROR_STREAM("error writing!");
}
Mat depthRead = imread(image_path2, CV_LOAD_IMAGE_ANYDEPTH);
FeatureExtractor FE(that->rgb_ptr->image ,depthRead);
FE.extract_feats();
FE.visualize_feats();
}
catch (cv::Exception& ex) {
fprintf(stderr, "Exception converting image to PNG format: %s\n", ex.what());
}
}
int main(int argc, char **argv) {
// Initialize the ROS system and become a node.
ros::init(argc, argv, "count_and_log");
ros::NodeHandle nh;
// Generate log messages of varying severity regularly
ros::Rate rate(10);
for (int i = 1; ros::ok(); i++) {
ROS_DEBUG_STREAM("Counted to " << i);
if((i % 3) == 0) {
ROS_INFO_STREAM(i << " is divisible by 3.");
}
if((i % 5) == 0) {
ROS_WARN_STREAM(i << " is divisivle by 5.");
}
if((i % 10) == 0) {
ROS_ERROR_STREAM(i << " is divisible by 10.");
}
if((i % 20) == 0) {
ROS_FATAL_STREAM(i << " is divisible by 20.");
}
rate.sleep();
}
}
void VrpnTrackerRos::init(std::string tracker_name, ros::NodeHandle nh, bool create_mainloop_timer)
{
ROS_INFO_STREAM("Creating new tracker " << tracker_name);
tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_pose);
tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_twist);
tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_accel);
tracker_remote_->shutup = true;
std::string error;
if (!ros::names::validate(tracker_name, error))
{
ROS_ERROR_STREAM("Invalid tracker name " << tracker_name << ", not creating topics : " << error);
return;
}
this->tracker_name = tracker_name;
output_nh_ = ros::NodeHandle(nh, tracker_name);
std::string frame_id;
nh.param<std::string>("frame_id", frame_id, "world");
nh.param<bool>("use_server_time", use_server_time_, false);
nh.param<bool>("broadcast_tf", broadcast_tf_, false);
nh.param<bool>("process_sensor_id", process_sensor_id_, false);
pose_msg_.header.frame_id = twist_msg_.header.frame_id = accel_msg_.header.frame_id = transform_stamped_.header.frame_id = frame_id;
if (create_mainloop_timer)
{
double update_frequency;
nh.param<double>("update_frequency", update_frequency, 100.0);
mainloop_timer = nh.createTimer(ros::Duration(1 / update_frequency),
boost::bind(&VrpnTrackerRos::mainloop, this));
}
}
bool registerPlanningScene(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &res)
{
register_lock_.lock();
if(req.__connection_header->find("callerid") == req.__connection_header->end()) {
ROS_ERROR_STREAM("Request has no callerid");
return false;
}
std::string callerid = req.__connection_header->find("callerid")->second;
if(sync_planning_scene_clients_.find(callerid) != sync_planning_scene_clients_.end()) {
delete sync_planning_scene_clients_[callerid];
}
sync_planning_scene_clients_[callerid] = new actionlib::SimpleActionClient<arm_navigation_msgs::SyncPlanningSceneAction>(callerid+"/"+SYNC_PLANNING_SCENE_NAME, true);
if(!sync_planning_scene_clients_[callerid]->waitForServer(ros::Duration(10.0))) {
ROS_INFO_STREAM("Couldn't connect back to action server for " << callerid << ". Removing from list");
delete sync_planning_scene_clients_[callerid];
sync_planning_scene_clients_.erase(callerid);
register_lock_.unlock();
return false;
}
ROS_INFO_STREAM("Successfully connected to planning scene action server for " << callerid);
register_lock_.unlock();
return true;
}
void move_to_wait_position(move_group_interface::MoveGroup& move_group)
{
//ROS_ERROR_STREAM("move_to_wait_position is not implemented yet. Aborting."); exit(1);
// task variables
bool success; // saves the move result
// set robot wait target
/* Fill Code: [ use the 'setNamedTarget' method in the 'move_group' object] */
move_group.setNamedTarget(cfg.WAIT_POSE_NAME);
// move the robot
/* Fill Code: [ use the 'move' method in the 'move_group' object and save the result in the 'success' variable] */
success = move_group.move();
if(success)
{
ROS_INFO_STREAM("Move " << cfg.WAIT_POSE_NAME<< " Succeeded");
}
else
{
ROS_ERROR_STREAM("Move " << cfg.WAIT_POSE_NAME<< " Failed");
exit(1);
}
}
void TinkerforgeSensors::publishTemperatureMessage(SensorDevice *sensor)
{
if (sensor != NULL)
{
int16_t temperature;
if(temperature_get_temperature((Temperature*)sensor->getDev(), &temperature) < 0) {
ROS_ERROR_STREAM("Could not get temperature from " << sensor->getUID() << ", probably timeout");
return;
}
// generate Temperature message from temperature sensor
sensor_msgs::Temperature temp_msg;
// message header
temp_msg.header.seq = sensor->getSeq();
temp_msg.header.stamp = ros::Time::now();
temp_msg.header.frame_id = sensor->getFrame();
temp_msg.temperature = temperature / 100.0;
temp_msg.variance = 0;
// publish Temperature msg to ros
sensor->getPub().publish(temp_msg);
}
}
int Cp1616IOController::changePnioMode(PNIO_MODE_TYPE requested_mode)
{
PNIO_UINT32 error_code;
PNIO_UINT32 valid_cp_handle = cp_handle_;
//set required mode
error_code = PNIO_set_mode(cp_handle_, requested_mode);
if(error_code != PNIO_OK)
{
ROS_ERROR_STREAM("Not able to change IO_Controller mode: Error 0x%x" << (int)error_code);
PNIO_close(cp_handle_); //Close PNIO_Controller
return (int)error_code;
}
if(cp_handle_ == valid_cp_handle) //check if cp_handle_ still valid
{
//wait for a callback_for_mode_change_indication
while(!sem_mod_change_){
usleep(WAIT_FOR_CALLBACKS_PERIOD);
}
setSemModChange(0);
}
//check if the current mode is correct
if(cp_current_mode_ != requested_mode)
{
ROS_ERROR("Not able to set required mode: ERROR another mode recieved");
PNIO_close(cp_handle_);
}
else
ROS_INFO("Changing IO_controller mode: done");
return (int)error_code;
}
bool TeleOpJoypad::moveGripper(std::string joint_position_name)
{
brics_actuator::JointPositions pos;
XmlRpc::XmlRpcValue position_list;
std::string param_name = "/script_server/gripper/" + joint_position_name;
// get gripper values
if(!nh_->getParam(param_name, position_list))
{
ROS_ERROR_STREAM("Could not find parameter <<" << param_name << " on parameter server");
return false;
}
ROS_ASSERT(position_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(position_list.size() == 1);
ROS_ASSERT(position_list[0].size() == 2);
// establish messgage
brics_actuator::JointValue gripper_left;
gripper_left.joint_uri = "gripper_finger_joint_l";
gripper_left.unit = "m";
gripper_left.value = static_cast<double>(position_list[0][0]);
brics_actuator::JointValue gripper_right;
gripper_right.joint_uri = "gripper_finger_joint_r";
gripper_right.unit = "m";
gripper_right.value = static_cast<double>(position_list[0][1]);
pos.positions.push_back(gripper_left);
pos.positions.push_back(gripper_right);
pub_gripper_position_.publish(pos);
return true;
}
/** Main driver loop */
bool spin()
{
while (nh_.ok())
{
getParameters(); // check reconfigurable parameters
// get current CameraInfo data
cam_info_ = cinfo_->getCameraInfo();
cloud2_.header.frame_id = cloud_.header.frame_id =
image_d_.header.frame_id = image_i_.header.frame_id =
image_c_.header.frame_id = image_d16_.header.frame_id =
cam_info_.header.frame_id = camera_name_;//config_.frame_id;
if(!device_open_)
{
try
{
if (dev_->open(config_.auto_exposure, config_.integration_time,
modulation_freq_, config_.amp_threshold, ether_addr_) == 0)
{
ROS_INFO_STREAM("[" << camera_name_ << "] Connected to device with ID: "
<< dev_->device_id_);
ROS_INFO_STREAM("[" << camera_name_ << "] libmesasr version: " << dev_->lib_version_);
device_open_ = true;
}
else
{
ros::Duration(3.0).sleep();
}
}
catch (sr::Exception& e)
{
ROS_ERROR_STREAM("Exception thrown while connecting to the camera: "
<< e.what ());
ros::Duration(3.0).sleep();
}
}
else
{
try
{
// Read data from the Camera
dev_->readData(cloud_,cloud2_,image_d_, image_i_, image_c_, image_d16_);
cam_info_.header.stamp = image_d_.header.stamp;
cam_info_.height = image_d_.height;
cam_info_.width = image_d_.width;
// Publish it via image_transport
if (info_pub_.getNumSubscribers() > 0)
info_pub_.publish(cam_info_);
if (image_pub_d_.getNumSubscribers() > 0)
image_pub_d_.publish(image_d_);
if (image_pub_i_.getNumSubscribers() > 0)
image_pub_i_.publish(image_i_);
if (image_pub_c_.getNumSubscribers() > 0)
image_pub_c_.publish(image_c_);
if (image_pub_d16_.getNumSubscribers() > 0)
image_pub_d16_.publish(image_d16_);
if (cloud_pub_.getNumSubscribers() > 0)
cloud_pub_.publish (cloud_);
if (cloud_pub2_.getNumSubscribers() > 0)
cloud_pub2_.publish (cloud2_);
}
catch (sr::Exception& e) {
ROS_WARN("Exception thrown trying to read data: %s",
e.what());
dev_->close();
device_open_ = false;
ros::Duration(3.0).sleep();
}
}
ros::spinOnce();
}
return true;
}
/***
* Starts up a SimpleGraspControlServer.
* Launch this node with the following launch file (or include it in another launch file):
*
* `` \`rospack find grasp_execution\`/launch/simple_grasp_control_server.launch ``
*
* Please also refer to this file (and the SimpleGraspControlServer header documentation)
* for more details about the required parameters.
*
* \author Jennifer Buehler
* \date March 2016
*/
int main(int argc, char**argv){
ros::init(argc, argv, "simple_grasp_action");
ros::NodeHandle priv("~");
ros::NodeHandle pub;
std::string JOINT_STATES_TOPIC="/joint_states";
priv.param<std::string>("joint_states_topic", JOINT_STATES_TOPIC, JOINT_STATES_TOPIC);
std::string JOINT_CONTROL_TOPIC="/joint_control";
priv.param<std::string>("joint_control_topic", JOINT_CONTROL_TOPIC, JOINT_CONTROL_TOPIC);
std::string GRASP_ACTION_TOPIC="/grasp_control_action";
priv.param<std::string>("grasp_control_action_topic", GRASP_ACTION_TOPIC, GRASP_ACTION_TOPIC);
std::string ROBOT_NAMESPACE;
if (!priv.hasParam("robot_namespace"))
{
ROS_ERROR_STREAM(ros::this_node::getName()<<": Must have at least 'robot_namespace' defined in private node namespace");
return 0;
}
priv.param<std::string>("robot_namespace", ROBOT_NAMESPACE, ROBOT_NAMESPACE);
double CHECK_FINGER_STATE_RATE=DEFAULT_CHECK_FINGER_STATE_RATE;
priv.param<double>("check_movement_rate", CHECK_FINGER_STATE_RATE, CHECK_FINGER_STATE_RATE);
double NO_MOVE_TOLERANCE=DEFAULT_NO_MOVE_TOLERANCE;
priv.param<double>("no_move_tolerance", NO_MOVE_TOLERANCE, NO_MOVE_TOLERANCE);
int NO_MOVE_STILL_CNT=DEFAULT_NO_MOVE_STILL_CNT;
priv.param<int>("no_move_still_cnt", NO_MOVE_STILL_CNT, NO_MOVE_STILL_CNT);
double GOAL_TOLERANCE=DEFAULT_GOAL_TOLERANCE;
priv.param<double>("goal_tolerance", GOAL_TOLERANCE, GOAL_TOLERANCE);
ROS_INFO("Launching arm components name manager");
arm_components_name_manager::ArmComponentsNameManager jointsManager(ROBOT_NAMESPACE, false);
float maxWait=5;
ROS_INFO("Waiting for joint info parameters to be loaded...");
if (!jointsManager.waitToLoadParameters(1,maxWait,1))
{
ROS_ERROR("Joint names (ArmComponentsNameManager) could not be launched due to missing ROS parameters.");
return 0;
}
grasp_execution::SimpleGraspControlServer actionServer(
pub,
GRASP_ACTION_TOPIC,
JOINT_STATES_TOPIC,
JOINT_CONTROL_TOPIC,
jointsManager,
GOAL_TOLERANCE,
NO_MOVE_TOLERANCE,
NO_MOVE_STILL_CNT,
CHECK_FINGER_STATE_RATE);
actionServer.init();
// ros::MultiThreadedSpinner spinner(4); // Use 4 threads
// spinner.spin(); // spin() will not return until the node has been shutdown
ros::spin();
return 0;
}
SlamNode::SlamNode(void)
{
ros::NodeHandle prvNh("~");
int iVar = 0;
double gridPublishInterval = 0.0;
double loopRateVar = 0.0;
double truncationRadius = 0.0;
double cellSize = 0.0;
unsigned int octaveFactor = 0;
double xOffset = 0.0;
double yOffset = 0.0;
std::string topicLaser;
prvNh.param<int>("robot_nbr", iVar, 1);
unsigned int robotNbr = static_cast<unsigned int>(iVar);
prvNh.param<double>("x_off_factor", _xOffFactor, 0.5);
prvNh.param<double>("y_off_factor", _yOffFactor, 0.5);
prvNh.param<double>("x_offset", xOffset, 0.0);
prvNh.param<double>("y_offset", yOffset, 0.0);
prvNh.param<int>("map_size", iVar, 10);
octaveFactor = static_cast<unsigned int>(iVar);
prvNh.param<double>("cellsize", cellSize, 0.025);
prvNh.param<int>("truncation_radius", iVar, 3);
truncationRadius = static_cast<double>(iVar);
prvNh.param<double>("occ_grid_time_interval", gridPublishInterval, 2.0);
prvNh.param<double>("loop_rate", loopRateVar, 40.0);
prvNh.param<std::string>("laser_topic", topicLaser, "scan");
_loopRate = new ros::Rate(loopRateVar);
_gridInterval = new ros::Duration(gridPublishInterval);
if(octaveFactor > 15)
{
ROS_ERROR_STREAM("Error! Unknown map size -> set to default!" << std::endl);
octaveFactor = 10;
}
//instanciate representation
_grid = new obvious::TsdGrid(cellSize, obvious::LAYOUT_32x32, static_cast<obvious::EnumTsdGridLayout>(octaveFactor)); //obvious::LAYOUT_8192x8192
_grid->setMaxTruncation(truncationRadius * cellSize);
unsigned int cellsPerSide = pow(2, octaveFactor);
double sideLength = static_cast<double>(cellsPerSide) * cellSize;
ROS_INFO_STREAM("Creating representation with " << cellsPerSide << "x" << cellsPerSide << "cells, representating " <<
sideLength << "x" << sideLength << "m^2" << std::endl);
//instanciate mapping threads
_threadMapping = new ThreadMapping(_grid);
_threadGrid = new ThreadGrid(_grid, &_nh, xOffset, yOffset);
ThreadLocalize* threadLocalize = NULL;
ros::Subscriber subs;
std::string nameSpace = "";
//instanciate localization threads
if(robotNbr == 1) //single slam
{
threadLocalize = new ThreadLocalize(_grid, _threadMapping, &_nh, nameSpace, xOffset, yOffset);
subs = _nh.subscribe(topicLaser, 1, &ThreadLocalize::laserCallBack, threadLocalize);
_subsLaser.push_back(subs);
_localizers.push_back(threadLocalize);
ROS_INFO_STREAM("Single SLAM started" << std::endl);
}
else
{
for(unsigned int i = 0; i < robotNbr; i++) //multi slam
{
std::stringstream sstream;
sstream << "robot";
sstream << i << "/namespace";
std::string dummy = sstream.str();
prvNh.param(dummy, nameSpace, std::string("default_ns"));
threadLocalize = new ThreadLocalize(_grid, _threadMapping, &_nh, nameSpace, xOffset, yOffset);
subs = _nh.subscribe(nameSpace + "/" + topicLaser, 1, &ThreadLocalize::laserCallBack, threadLocalize);
_subsLaser.push_back(subs);
_localizers.push_back(threadLocalize);
ROS_INFO_STREAM("started for thread for " << nameSpace << std::endl);
}
ROS_INFO_STREAM("Multi SLAM started!");
}
}
int main(int argc, char **argv)
{
if (argc < 2)
{
ROS_ERROR_STREAM("Please provide the name of the reachability map. If you have not created it yet, Please create "
"the map by running the create reachability map launch file in map_creator");
return 1;
}
else
{
ros::init(argc, argv, "workspace");
ros::NodeHandle n;
// TODO: It can be published as a latched topic. So the whole message will be published just once and stay on the
// topic
ros::Publisher workspace_pub = n.advertise< map_creator::WorkSpace >("reachability_map", 1);
// bool latchOn = 1;
// ros::Publisher workspace_pub = n.advertise<map_creator::WorkSpace>("reachability_map", 1, latchOn);
ros::Rate loop_rate(10);
int count = 0;
hdf5_dataset::Hdf5Dataset h5(argv[1]);
h5.open();
MultiMapPtr pose_col_filter;
MapVecDoublePtr sphere_col;
float res;
h5.loadMapsFromDataset(pose_col_filter, sphere_col, res);
// Creating messages
map_creator::WorkSpace ws;
ws.header.stamp = ros::Time::now();
ws.header.frame_id = "/base_link";
ws.resolution = res;
for (MapVecDoublePtr::iterator it = sphere_col.begin(); it != sphere_col.end(); ++it)
{
map_creator::WsSphere wss;
wss.point.x = (*it->first)[0];
wss.point.y = (*it->first)[1];
wss.point.z = (*it->first)[2];
wss.ri = it->second;
for (MultiMapPtr::iterator it1 = pose_col_filter.lower_bound(it->first); it1 != pose_col_filter.upper_bound(it->first); ++it1)
{
geometry_msgs::Pose pp;
pp.position.x = it1->second->at(0);
pp.position.y = it1->second->at(1);
pp.position.z = it1->second->at(2);
pp.orientation.x = it1->second->at(3);
pp.orientation.y = it1->second->at(4);
pp.orientation.z = it1->second->at(5);
pp.orientation.w = it1->second->at(6);
wss.poses.push_back(pp);
}
ws.WsSpheres.push_back(wss);
}
while (ros::ok())
{
workspace_pub.publish(ws);
ros::spinOnce();
sleep(5);
++count;
}
}
return 0;
}
bool PerceptionPlugin::srvClassify(ed_perception::Classify::Request& req, ed_perception::Classify::Response& res)
{
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Configure classifier
if (!configureClassifier(req.perception_models_path, res.error_msg))
{
ROS_ERROR_STREAM(res.error_msg);
return true;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Convert prior msg to distribution
CategoricalDistribution prior;
for(unsigned int i = 0; i < req.prior.values.size(); ++i)
prior.setScore(req.prior.values[i], req.prior.probabilities[i]);
prior.setUnknownScore(req.prior.unknown_probability);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Classify
for(std::vector<std::string>::const_iterator it = req.ids.begin(); it != req.ids.end(); ++it)
{
ed::EntityConstPtr e = world_->getEntity(*it);
if (!e)
{
res.error_msg += "Entity '" + *it + "' does not exist.\n";
ROS_ERROR_STREAM(res.error_msg);
continue;
}
if (!e->bestMeasurement())
{
res.error_msg += "Entity '" + *it + "' does not have a measurement.\n";
ROS_ERROR_STREAM(res.error_msg);
continue;
}
tue::Configuration data;
ed::perception::ClassificationOutput output(data);
aggregator_.classify(*e, req.property, prior, output);
// TODO: create posterior! (is now just likelihood)
ed::perception::CategoricalDistribution posterior = output.likelihood;
posterior.normalize();
// - - - - - - - - - - - - - - - - - - - - - -
// Write result to message
res.ids.push_back(e->id().str());
res.posteriors.push_back(ed_perception::CategoricalDistribution());
ed_perception::CategoricalDistribution& dist_msg = res.posteriors.back();
for(std::map<std::string, double>::const_iterator it2 = posterior.values().begin(); it2 != posterior.values().end(); ++it2)
{
dist_msg.values.push_back(it2->first);
dist_msg.probabilities.push_back(it2->second);
}
dist_msg.unknown_probability = posterior.getUnknownScore();
std::string max_value;
double max_prob;
if (posterior.getMaximum(max_value, max_prob) && max_prob > posterior.getUnknownScore())
{
res.expected_values.push_back(max_value);
res.expected_value_probabilities.push_back(max_prob);
// If the classification property is 'type', set the type of the entity in the world model
if (req.property == "type")
{
update_req_->setType(e->id(), max_value);
update_req_->addData(e->id(), data.data());
}
}
else
{
res.expected_values.push_back("");
res.expected_value_probabilities.push_back(posterior.getUnknownScore());
}
}
return true;
}
bool FollowJointTrajectoryController::init(ros::NodeHandle& nh, ControllerManager* manager)
{
/* We absolutely need access to the controller manager */
if (!manager)
{
initialized_ = false;
return false;
}
Controller::init(nh, manager);
/* No initial sampler */
boost::mutex::scoped_lock lock(sampler_mutex_);
sampler_.reset();
preempted_ = false;
/* Get Joint Names */
joint_names_.clear();
XmlRpc::XmlRpcValue names;
if (!nh.getParam("joints", names))
{
ROS_ERROR_STREAM("No joints given for " << nh.getNamespace());
return false;
}
if (names.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR_STREAM("Joints not in a list for " << nh.getNamespace());
return false;
}
for (int i = 0; i < names.size(); ++i)
{
XmlRpc::XmlRpcValue &name_value = names[i];
if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
{
ROS_ERROR_STREAM("Not all joint names are strings for " << nh.getNamespace());
return false;
}
joint_names_.push_back(static_cast<std::string>(name_value));
}
/* Get parameters */
nh.param<bool>("stop_with_action", stop_with_action_, false);
/* Get Joint Handles, setup feedback */
joints_.clear();
for (size_t i = 0; i < joint_names_.size(); ++i)
{
JointHandle* j = manager_->getJointHandle(joint_names_[i]);
feedback_.joint_names.push_back(j->getName());
joints_.push_back(j);
}
/* Update feedback */
feedback_.desired.positions.resize(joints_.size());
feedback_.desired.velocities.resize(joints_.size());
feedback_.desired.accelerations.resize(joints_.size());
feedback_.actual.positions.resize(joints_.size());
feedback_.actual.velocities.resize(joints_.size());
feedback_.actual.effort.resize(joints_.size());
feedback_.error.positions.resize(joints_.size());
feedback_.error.velocities.resize(joints_.size());
/* Update tolerances */
path_tolerance_.q.resize(joints_.size());
path_tolerance_.qd.resize(joints_.size());
path_tolerance_.qdd.resize(joints_.size());
goal_tolerance_.q.resize(joints_.size());
goal_tolerance_.qd.resize(joints_.size());
goal_tolerance_.qdd.resize(joints_.size());
/* Setup ROS interfaces */
server_.reset(new server_t(nh, "",
boost::bind(&FollowJointTrajectoryController::executeCb, this, _1),
false));
server_->start();
initialized_ = true;
return true;
}
bool TaskInverseKinematics::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
{
nh_ = n;
// get URDF and name of root and tip from the parameter server
std::string robot_description, root_name, tip_name;
if (!ros::param::search(n.getNamespace(),"robot_description", robot_description))
{
ROS_ERROR_STREAM("TaskInverseKinematics: No robot description (URDF) found on parameter server ("<<n.getNamespace()<<"/robot_description)");
return false;
}
if (!nh_.getParam("root_name", root_name))
{
ROS_ERROR_STREAM("TaskInverseKinematics: No root name found on parameter server ("<<n.getNamespace()<<"/root_name)");
return false;
}
if (!nh_.getParam("tip_name", tip_name))
{
ROS_ERROR_STREAM("TaskInverseKinematics: No tip name found on parameter server ("<<n.getNamespace()<<"/tip_name)");
return false;
}
ROS_INFO("robot_description: %s",robot_description.c_str());
ROS_INFO("root_name: %s",root_name.c_str());
ROS_INFO("tip_name: %s",tip_name.c_str());
// Get the gravity vector (direction and magnitude)
KDL::Vector gravity_ = KDL::Vector::Zero();
gravity_(2) = -9.81;
// Construct an URDF model from the xml string
std::string xml_string;
if (n.hasParam(robot_description))
n.getParam(robot_description.c_str(), xml_string);
else
{
ROS_ERROR("Parameter %s not set, shutting down node...", robot_description.c_str());
n.shutdown();
return false;
}
if (xml_string.size() == 0)
{
ROS_ERROR("Unable to load robot model from parameter %s",robot_description.c_str());
n.shutdown();
return false;
}
ROS_DEBUG("%s content\n%s", robot_description.c_str(), xml_string.c_str());
// Get urdf model out of robot_description
urdf::Model model;
if (!model.initString(xml_string))
{
ROS_ERROR("Failed to parse urdf file");
n.shutdown();
return false;
}
ROS_DEBUG("Successfully parsed urdf file");
KDL::Tree kdl_tree_;
if (!kdl_parser::treeFromUrdfModel(model, kdl_tree_))
{
ROS_ERROR("Failed to construct kdl tree");
n.shutdown();
return false;
}
// Populate the KDL chain
if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
{
ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
ROS_ERROR_STREAM(" "<<root_name<<" --> "<<tip_name);
ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfJoints()<<" joints");
ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfSegments()<<" segments");
ROS_ERROR_STREAM(" The segments are:");
KDL::SegmentMap segment_map = kdl_tree_.getSegments();
KDL::SegmentMap::iterator it;
for( it=segment_map.begin(); it != segment_map.end(); it++ )
ROS_ERROR_STREAM( " "<<(*it).first);
return false;
}
ROS_DEBUG("Number of segments: %d", kdl_chain_.getNrOfSegments());
ROS_DEBUG("Number of joints in chain: %d", kdl_chain_.getNrOfJoints());
// Parsing joint limits from urdf model
boost::shared_ptr<const urdf::Link> link_ = model.getLink(tip_name);
boost::shared_ptr<const urdf::Joint> joint_;
joint_limits_.min.resize(kdl_chain_.getNrOfJoints());
joint_limits_.max.resize(kdl_chain_.getNrOfJoints());
joint_limits_.center.resize(kdl_chain_.getNrOfJoints());
int index;
std::cout<< "kdl_chain_.getNrOfJoints(): " << kdl_chain_.getNrOfJoints() << std::endl;
for (int i = 0; i < kdl_chain_.getNrOfJoints() && link_; i++)
{
joint_ = model.getJoint(link_->parent_joint->name);
index = kdl_chain_.getNrOfJoints() - i - 1;
joint_limits_.min(index) = joint_->limits->lower;
joint_limits_.max(index) = joint_->limits->upper;
joint_limits_.center(index) = (joint_limits_.min(index) + joint_limits_.max(index))/2;
link_ = model.getLink(link_->getParent()->name);
}
ROS_INFO("Successfully parsed joint limits");
// Get joint handles for all of the joints in the chain
for(std::vector<KDL::Segment>::const_iterator it = kdl_chain_.segments.begin()+1; it != kdl_chain_.segments.end(); ++it)
{
joint_handles_.push_back(robot->getHandle(it->getJoint().getName()));
ROS_DEBUG("%s", it->getJoint().getName().c_str() );
}
ROS_DEBUG(" Number of joints in handle = %lu", joint_handles_.size() );
PIDs_.resize(kdl_chain_.getNrOfJoints());
// Parsing PID gains from YAML
std::string pid_ = ("pid_");
for (int i = 0; i < joint_handles_.size(); ++i)
{
if (!PIDs_[i].init(ros::NodeHandle(n, pid_ + joint_handles_[i].getName())))
{
ROS_ERROR("Error initializing the PID for joint %d",i);
return false;
}
}
jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
id_solver_.reset(new KDL::ChainDynParam(kdl_chain_,gravity_));
fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(kdl_chain_));
ik_pos_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(kdl_chain_,joint_limits_.min,joint_limits_.max,*fk_pos_solver_,*ik_vel_solver_));
joint_msr_states_.resize(kdl_chain_.getNrOfJoints());
joint_des_states_.resize(kdl_chain_.getNrOfJoints());
tau_cmd_.resize(kdl_chain_.getNrOfJoints());
J_.resize(kdl_chain_.getNrOfJoints());
sub_command_ = nh_.subscribe("command_configuration", 1, &TaskInverseKinematics::command_configuration, this);
return true;
}
void NavSatTransform::run()
{
ros::Time::init();
double frequency = 10.0;
double delay = 0.0;
ros::NodeHandle nh;
ros::NodeHandle nhPriv("~");
// Load the parameters we need
nhPriv.getParam("magnetic_declination_radians", magneticDeclination_);
nhPriv.param("yaw_offset", yawOffset_, 0.0);
nhPriv.param("broadcast_utm_transform", broadcastUtmTransform_, false);
nhPriv.param("zero_altitude", zeroAltitude_, false);
nhPriv.param("publish_filtered_gps", publishGps_, false);
nhPriv.param("use_odometry_yaw", useOdometryYaw_, false);
nhPriv.param("wait_for_datum", useManualDatum_, false);
nhPriv.param("frequency", frequency, 10.0);
nhPriv.param("delay", delay, 0.0);
// Subscribe to the messages and services we need
ros::ServiceServer datumServ = nh.advertiseService("datum", &NavSatTransform::datumCallback, this);
if(useManualDatum_ && nhPriv.hasParam("datum"))
{
XmlRpc::XmlRpcValue datumConfig;
try
{
double datumLat;
double datumLon;
double datumYaw;
nhPriv.getParam("datum", datumConfig);
ROS_ASSERT(datumConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(datumConfig.size() == 4);
useManualDatum_ = true;
std::ostringstream ostr;
ostr << datumConfig[0] << " " << datumConfig[1] << " " << datumConfig[2] << " " << datumConfig[3];
std::istringstream istr(ostr.str());
istr >> datumLat >> datumLon >> datumYaw >> worldFrameId_;
robot_localization::SetDatum::Request request;
request.geo_pose.position.latitude = datumLat;
request.geo_pose.position.longitude = datumLon;
request.geo_pose.position.altitude = 0.0;
tf2::Quaternion quat;
quat.setRPY(0.0, 0.0, datumYaw);
request.geo_pose.orientation = tf2::toMsg(quat);
robot_localization::SetDatum::Response response;
datumCallback(request, response);
}
catch (XmlRpc::XmlRpcException &e)
{
ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() <<
" for process_noise_covariance (type: " << datumConfig.getType() << ")");
}
}
sptam::sptam_node::sptam_node(ros::NodeHandle& nh, ros::NodeHandle& nhp)
:
sptam_(nullptr),
motionModel_(new MotionModel(cv::Point3d(0,0,0), cv::Vec4d(1,0,0,0))),
it_( nhp ),
odom_to_map_(tf::Transform::getIdentity()),
transform_thread_(nullptr)
{
// Get node parameters
nhp.param<std::string>("odom_frame", odom_frame_, "/odom");
nhp.param<std::string>("base_link_frame", base_frame_, "/base_link");
nhp.param<std::string>("camera_frame", camera_frame_, "/camera");
nhp.param<std::string>("map_frame", map_frame_, "/map");
nhp.param<bool>("use_odometry", use_odometry_, false);
nhp.param<double>("transform_publish_freq", transform_publish_freq_, 30.0);
nhp.param<double>("tf_delay", tf_delay_, 1.0/transform_publish_freq_);
bool use_approx_sync;
nhp.param<bool>("approximate_sync", use_approx_sync, false);
// load feature detector
{
std::string detectorName;
nhp.param<std::string>("FeatureDetector/Name", detectorName, "GFTT");
std::cout << "detector: " << detectorName << std::endl;
featureDetector_ = cv::FeatureDetector::create( detectorName );
if ( not featureDetector_ )
ROS_ERROR_STREAM("could not load feature detector with name " << detectorName);
setParameters( nhp, featureDetector_, "FeatureDetector" );
}
// load descriptor extractor
{
std::string extractorName;
nhp.param<std::string>("DescriptorExtractor/Name", extractorName, "BRIEF");
std::cout << "extractor: " << extractorName << std::endl;
descriptorExtractor_ = cv::DescriptorExtractor::create( extractorName );
if ( not descriptorExtractor_ )
ROS_ERROR_STREAM("could not load descriptor extractor with name " << extractorName);
setParameters( nhp, descriptorExtractor_, "DescriptorExtractor" );
}
// load descriptor matcher
{
std::string matcherName;
nhp.param<std::string>("DescriptorMatcher/Name", matcherName, "BruteForce-Hamming");
std::cout << "matcher: " << matcherName << std::endl;
mapper_params_.descriptorMatcher = cv::DescriptorMatcher::create( matcherName );
if ( not mapper_params_.descriptorMatcher )
ROS_ERROR_STREAM("could not load descriptor matcher with name " << matcherName);
setParameters( nhp, mapper_params_.descriptorMatcher, "DescriptorMatcher" );
}
// Mapper Parameters
// nhp.param is not overloaded for unsigned int
int matchingCellSizeParam, framesBetweenKeyFramesParam, matchingNeighborhoodParam;
nhp.param<int>("MatchingCellSize", matchingCellSizeParam, 30);
mapper_params_.matchingCellSize = matchingCellSizeParam;
nhp.param<double>("MatchingDistance", mapper_params_.matchingDistanceThreshold, 25.0);
nhp.param<int>("MatchingNeighborhood", matchingNeighborhoodParam, 1);
mapper_params_.matchingNeighborhoodThreshold = matchingNeighborhoodParam;
nhp.param<double>("EpipolarDistance", mapper_params_.epipolarDistanceThreshold, 0.0);
nhp.param<double>("KeyFrameDistance", mapper_params_.keyFrameDistanceThreshold, 0.0);
nhp.param<int>("FramesBetweenKeyFrames", framesBetweenKeyFramesParam, 0);
mapper_params_.framesBetweenKeyFrames = framesBetweenKeyFramesParam;
// Camera Calibration Parameters
nhp.param<double>("FrustumNearPlaneDist", cameraParametersLeft_.frustumNearPlaneDist, 0.1);
nhp.param<double>("FrustumFarPlaneDist", cameraParametersLeft_.frustumFarPlaneDist, 1000.0);
cameraParametersRight_.frustumNearPlaneDist = cameraParametersLeft_.frustumNearPlaneDist;
cameraParametersRight_.frustumFarPlaneDist = cameraParametersLeft_.frustumFarPlaneDist;
// Create RowMatcher instance
rowMatcher_ = new RowMatcher( mapper_params_.matchingDistanceThreshold, mapper_params_.descriptorMatcher, mapper_params_.epipolarDistanceThreshold );
// Subscribe to images messages
sub_l_image_.subscribe(nhp, "/stereo/left/image_rect", 1);
sub_l_info_ .subscribe(nhp, "/stereo/left/camera_info", 1);
sub_r_image_.subscribe(nhp, "/stereo/right/image_rect", 1);
sub_r_info_ .subscribe(nhp, "/stereo/right/camera_info", 1);
if ( use_approx_sync )
{
approximate_sync_.reset( new ApproximateSync( ApproximatePolicy(10),
sub_l_image_, sub_l_info_,
sub_r_image_, sub_r_info_ ) );
approximate_sync_->registerCallback( boost::bind( &sptam::sptam_node::onImages,
this, _1, _2, _3, _4 ) );
}
else
{
exact_sync_.reset( new ExactSync( ExactPolicy(1),
sub_l_image_, sub_l_info_,
sub_r_image_, sub_r_info_ ) );
exact_sync_->registerCallback( boost::bind( &sptam::sptam_node::onImages,
this, _1, _2, _3, _4 ) );
}
mapPub_ = nhp.advertise<sensor_msgs::PointCloud2>("point_cloud", 100);
posePub_ = nhp.advertise<geometry_msgs::PoseStamped>("robot/pose", 100);
leftKpPub_ = it_.advertise("/stereo/left/keypoints", 1);
rightKpPub_ = it_.advertise("/stereo/right/keypoints", 1);
// start map->odom transform publisher thread
if ( use_odometry_ )
// this loop periodically publishes map->odom transform from another thread
transform_thread_ = new std::thread( boost::bind(&sptam::sptam_node::publishTransformLoop, this) );
else
// this loop periodically publishes map->base_link transform from another thread
transform_thread_ = new std::thread( boost::bind(&sptam::sptam_node::publishTransformLoop2, this) );
ROS_INFO_STREAM("sptam node initialized");
}
/**
* @brief retransform map to the new time frame
* @param currentTime time frame to transform to
*/
void srs_env_model::CCMapPlugin::retransformTF( const ros::Time & currentTime )
{
// Listen and store current collision map transform matrix
tf::StampedTransform lockedToCurrentTF;
try
{
m_tfListener.waitForTransform( m_cmapFrameId, currentTime, m_cmapFrameId, m_mapTime, FIXED_FRAME, ros::Duration(5) );
m_tfListener.lookupTransform( m_cmapFrameId, currentTime, m_cmapFrameId, m_mapTime, FIXED_FRAME, lockedToCurrentTF );
}
catch (tf::TransformException& ex) {
ROS_ERROR_STREAM("Transform error: " << ex.what() << ", quitting callback");
PERROR( "Transform error.");
return;
}
Eigen::Matrix4f m;
// Get transform matrix
pcl_ros::transformAsMatrix( lockedToCurrentTF, m );
// Transform data front buffer
tBoxVec::iterator it, itEnd( m_data->boxes.end() );
for( it = m_data->boxes.begin(); it != itEnd; ++it )
{
// Copy values to the eigen vectors
Eigen::Vector4f position( it->center.x, it->center.y, it->center.z, 1.0f );
Eigen::Vector4f extents( it->extents.x, it->extents.y, it->extents.z, 1.0f );
extents += position;
// Retransform
position = m * position;
extents = m * extents;
// Copy values back to the box
it->center.x = position[0];
it->center.y = position[1];
it->center.z = position[2];
// Recompute extents
extents = extents - position;
it->extents.x = abs(extents[0]);
it->extents.y = abs(extents[1]);
it->extents.z = abs(extents[2]);
}
// Transform back buffer
itEnd = m_dataBuffer->boxes.end();
for( it = m_dataBuffer->boxes.begin(); it != itEnd; ++it )
{
// Copy values to the eigen vectors
Eigen::Vector4f position( it->center.x, it->center.y, it->center.z, 1.0f );
Eigen::Vector4f extents( it->extents.x, it->extents.y, it->extents.z, 1.0f );
extents += position;
// Retransform
position = m * position;
extents = m * extents;
// Copy values back to the box
it->center.x = position[0];
it->center.y = position[1];
it->center.z = position[2];
// Recompute extents
extents -= position;
it->extents.x = abs(extents[0]);
it->extents.y = abs(extents[1]);
it->extents.z = abs(extents[2]);
}
// Store timestamp
m_mapTime = currentTime;
}
bool SerialPort::setRate(long rate)
{
if(!_port_is_open)
{
switch (rate)
{
case 38400:
default:
_baudrate = B38400;
break;
case 19200:
_baudrate = B19200;
break;
case 9600:
_baudrate = B9600;
break;
case 4800:
_baudrate = B4800;
break;
case 2400:
_baudrate = B2400;
break;
case 1800:
_baudrate = B1800;
break;
case 1200:
_baudrate = B1200;
break;
case 600:
_baudrate = B600;
break;
case 300:
_baudrate = B300;
break;
case 200:
_baudrate = B200;
break;
case 150:
_baudrate = B150;
break;
case 134:
_baudrate = B134;
break;
case 110:
_baudrate = B110;
break;
case 75:
_baudrate = B75;
break;
case 50:
_baudrate = B50;
break;
}
}
else
{
ROS_ERROR_STREAM("Serial Port is already been used as name : "<<_port_name<<
". Hence the rate cannot be changed");
}
return !_port_is_open;
}
bool NtripStream::connect() {
if (_is_login) {
return true;
}
if (!_tcp_stream) {
ROS_ERROR("New tcp stream failed.");
return true;
}
if (!_tcp_stream->connect()) {
_status = Stream::Status::DISCONNECTED;
ROS_ERROR("Tcp connect failed.");
return false;
}
uint8_t buffer[2048];
size_t size = 0;
size_t try_times = 0;
size = _tcp_stream->write(
reinterpret_cast<const uint8_t*>(_login_data.data()), _login_data.size());
if (size != _login_data.size()) {
_tcp_stream->disconnect();
_status = Stream::Status::ERROR;
ROS_ERROR("Send ntrip request failed.");
return false;
}
bzero(buffer, sizeof(buffer));
ROS_INFO("Read ntrip response.");
size = _tcp_stream->read(buffer, sizeof(buffer) - 1);
while ((size == 0) && (try_times < 3)) {
sleep(1);
size = _tcp_stream->read(buffer, sizeof(buffer) - 1);
++try_times;
}
if (!size) {
_tcp_stream->disconnect();
_status = Stream::Status::DISCONNECTED;
ROS_ERROR("No response from ntripcaster.");
return false;
}
if (std::strstr(reinterpret_cast<char*>(buffer), "ICY 200 OK\r\n")) {
_status = Stream::Status::CONNECTED;
_is_login = true;
ROS_INFO("Ntrip login successfully.");
return true;
}
if (std::strstr(reinterpret_cast<char*>(buffer), "SOURCETABLE 200 OK\r\n")) {
ROS_ERROR_STREAM("Mountpoint " << _mountpoint << " not exist.");
}
if (std::strstr(reinterpret_cast<char*>(buffer), "HTTP/")) {
ROS_ERROR("Authentication failed.");
}
ROS_INFO_STREAM("No expect data.");
ROS_INFO_STREAM("Recv data length: " << size);
// ROS_INFO_STREAM("Data from server: " << reinterpret_cast<char*>(buffer));
_tcp_stream->disconnect();
_status = Stream::Status::ERROR;
return false;
}
