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; }