diff --git a/cfg/LidarLiteDriver.cfg b/cfg/LidarLiteDriver.cfg index a82728a635031435b1b0d1cb9acecdd87306c37b..cf71f91a3d2e0a5e967b78bd8fa965939101def6 100755 --- a/cfg/LidarLiteDriver.cfg +++ b/cfg/LidarLiteDriver.cfg @@ -40,6 +40,6 @@ gen = ParameterGenerator() # Name Type Reconfiguration level Description Default Min Max #gen.add("velocity_scale_factor", double_t, SensorLevels.RECONFIGURE_STOP, "Maximum velocity scale factor", 0.5, 0.0, 1.0) -gen.add("Config_mode", int_t, 0, "LidarLite configuration mode", 0, 1, 5) +gen.add("config_mode", int_t, 0, "LidarLite configuration mode", 0, 0, 5) exit(gen.generate(PACKAGE, "LidarLiteDriver", "LidarLiteDriver")) diff --git a/include/lidar_lite_driver.h b/include/lidar_lite_driver.h index a1a0d889173771fd5b2504c3067953ba2801c297..176afabffc85f1e6dca0784ea801b63b290f9d4d 100644 --- a/include/lidar_lite_driver.h +++ b/include/lidar_lite_driver.h @@ -30,8 +30,8 @@ #include <iri_base_driver/iri_base_driver.h> #include <iri_lidar_lite_driver/LidarLiteDriverConfig.h> -#include <iridrivers/lidar_lite.h> -#include <iridrivers/lidar_lite_exceptions.h> +#include <iridrivers/lidar_lite/lidar_lite.h> +#include <iridrivers/lidar_lite/lidar_lite_exceptions.h> /** * \brief IRI ROS Specific Driver Class diff --git a/launch/iri_lidar_lite.launch b/launch/iri_lidar_lite.launch index 57f50caa9df2a1370fd8c1189dc1e3010d5fbeb8..f953c7a22ecfd3590f09ed68351d90c7a578f314 100644 --- a/launch/iri_lidar_lite.launch +++ b/launch/iri_lidar_lite.launch @@ -2,10 +2,10 @@ <launch> - <node pkg="iri_lidar_lite" type="iri_lidar_lite" name="iri_lidar_lite" output="screen"> + <node pkg="iri_lidar_lite_driver" type="iri_lidar_lite_driver" name="iri_lidar_lite_driver" output="screen"> <param name="serial_num" type="string" value="A700evSl"/> <param name="config_mode" type="int" value="0"/> - <param name="frame_id" type="string" value="/kinton/lidarlite_link"/> + <param name="frame_id" type="string" value="/lidarlite_link"/> <remap from="~range" to="kinton/lidarlite_range"/> </node> diff --git a/src/lidar_lite_driver.cpp b/src/lidar_lite_driver.cpp index 16d448eafb6d63e1cb33ee2996233fd3b13ad6a7..c5b205806acb10152dda80a622f8fd82f6d7ca81 100644 --- a/src/lidar_lite_driver.cpp +++ b/src/lidar_lite_driver.cpp @@ -66,7 +66,7 @@ LidarLiteDriver::config_update(Config& new_cfg, uint32_t level) case iri_base_driver::RUNNING: - this->config_mode_ = new_cfg.Config_mode; + this->config_mode_ = new_cfg.config_mode; try { this->laser_ptr_->set_new_config(this->config_mode_); diff --git a/src/lidar_lite_driver_node.cpp b/src/lidar_lite_driver_node.cpp index 3ab56951a45a3217226a77040f0ba710762a9f90..fdc0a6959ef0b18dcb5e1ae862361c543819953a 100644 --- a/src/lidar_lite_driver_node.cpp +++ b/src/lidar_lite_driver_node.cpp @@ -7,15 +7,16 @@ LidarLiteDriverNode::LidarLiteDriverNode(ros::NodeHandle& nh) // , RunThread_(true) // , Thread_(boost::bind(&LidarLiteDriverNode::ThreadFunc, this)) { - this->setRate(30); + this->setRate(100); // Read from launch file std::string serial; int config_mode; - this->public_node_handle_.param<std::string>("serial_num", serial, - "A700evSl"); - this->public_node_handle_.param<int>("config_mode", config_mode, 0); - this->public_node_handle_.param<std::string>("frame_id", this->frame_id_, + ros::NodeHandle node_handle; + node_handle.param<std::string>("/iri_lidar_lite_driver/serial_num", serial, + ""); + node_handle.param<int>("/iri_lidar_lite_driver/config_mode", config_mode, 0); + node_handle.param<std::string>("/iri_lidar_lite_driver/frame_id", this->frame_id_, "lidar_lite"); // Set device parameters @@ -23,7 +24,7 @@ LidarLiteDriverNode::LidarLiteDriverNode(ros::NodeHandle& nh) // [init publishers] this->range_publisher_ = - this->public_node_handle_.advertise<sensor_msgs::Range>("range", 100); + this->public_node_handle_.advertise<sensor_msgs::Range>("lidarlite_range/range", 100); this->range_seq_ = 0; // [init subscribers]