From: Walter Fetter Lages Date: Sun, 11 Aug 2019 08:07:03 +0000 (-0300) Subject: Change topics to comply with ROS REP 145. X-Git-Tag: f1~3 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=0b00117a009a6efcea8aaf3062972828c49892ea;p=bno055_driver.git Change topics to comply with ROS REP 145. --- diff --git a/launch/display.launch b/launch/display.launch index 2956a4c..867b2c9 100644 --- a/launch/display.launch +++ b/launch/display.launch @@ -1,10 +1,10 @@ - - - + args="0 0 0 0 0 0 1 map imu_link" /> + + [0.0, 0.0, 0.0] + diff --git a/rviz/display.rviz b/rviz/display.rviz index 8738e7f..3b00f73 100644 --- a/rviz/display.rviz +++ b/rviz/display.rviz @@ -3,11 +3,8 @@ Panels: Help Height: 78 Name: Displays Property Tree Widget: - Expanded: - - /RobotModel1 - - /RobotModel1/Links1 - - /RobotModel1/Links1/base_link1 - Splitter Ratio: 0.3928571343421936 + Expanded: ~ + Splitter Ratio: 0.4301801919937134 Tree Height: 558 - Class: rviz/Selection Name: Selection @@ -27,7 +24,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: Temperature Preferences: PromptSaveOnExit: true Visualization Manager: @@ -52,47 +49,38 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false + Class: rviz_plugin_tutorials/Imu + Color: 204; 51; 204 Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: true - Show Trail: false - Value: true - origin_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 + History Length: 1 + Name: Imu (data) + Topic: /imu/data + Unreliable: false Value: true - Visual Enabled: true - - Alpha: 1 + - Alpha: 0.10000000149011612 Class: rviz_plugin_tutorials/Imu - Color: 204; 51; 204 + Color: 57; 52; 204 Enabled: true History Length: 1 - Name: Imu - Topic: /imu + Name: Imu (raw data) + Topic: /imu/data_raw Unreliable: false Value: true - Class: rviz/Axes Enabled: true - Length: 0.10000000149011612 - Name: Axes - Radius: 0.009999999776482582 + Length: 5 + Name: Axes (Fixed Frame) + Radius: 0.10000000149011612 Reference Frame: Value: true - - Alpha: 1 + - Class: rviz/Axes + Enabled: true + Length: 5 + Name: Axes (base_link) + Radius: 0.10000000149011612 + Reference Frame: base_link + Value: true + - Alpha: 0.30000001192092896 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 @@ -104,7 +92,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 Max Intensity: 100 @@ -115,13 +103,13 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.10000000149011612 + Size (m): 1 Style: Flat Squares - Topic: /temperature + Topic: /imu/temperature Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: false + Value: true Enabled: true Global Options: Background Color: 249; 249; 249 @@ -147,16 +135,16 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.749889850616455 + Distance: 31.31101417541504 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.19180631637573242 - Y: -0.2178049087524414 - Z: 0.2896389067173004 + X: -0.07251755893230438 + Y: -0.0821044072508812 + Z: 0.10933426022529602 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -173,7 +161,7 @@ Window Geometry: Height: 846 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001de000002b6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a000002b6000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003a000002b60000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055c0000003efc0100000002fb0000000800540069006d006501000000000000055c0000024200fffffffb0000000800540069006d00650100000000000004500000000000000000000002630000020d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001be000002b6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a000002b6000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002b6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003a000002b60000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055c0000003efc0100000002fb0000000800540069006d006501000000000000055c0000024200fffffffb0000000800540069006d00650100000000000004500000000000000000000002920000020d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -183,5 +171,5 @@ Window Geometry: Views: collapsed: false Width: 1372 - X: 32 + X: 16 Y: 0 diff --git a/src/bno055_node.cpp b/src/bno055_node.cpp index ab71e3e..024d863 100644 --- a/src/bno055_node.cpp +++ b/src/bno055_node.cpp @@ -33,6 +33,8 @@ #include using namespace Eigen; +#define sqr(x) ((x)*(x)) + class Bno055Node { public: @@ -44,10 +46,11 @@ class Bno055Node private: struct bno055_t bno055_; ros::NodeHandle node_; - ros::Publisher imuPub_; + ros::Publisher dataRawPub_; + ros::Publisher dataPub_; ros::Publisher temperaturePub_; ros::Publisher magneticFieldPub_; - ros::Publisher accelPub_; + ros::Publisher rpyPub_; ros::Publisher gravityPub_; bool uart_; @@ -55,12 +58,20 @@ class Bno055Node Vector3d accel_; Vector3d mag_; Vector3d gyro_; - + Quaterniond quaternion_; + Vector3d rpy_; Vector3d linearAccel_; Vector3d gravity_; - + + double accStdDev_; + double gyroStdDev_; + double magStdDev_; + double orientationStdDev_; + double temp_; + + std::string frameId_; }; Bno055Node::Bno055Node(ros::NodeHandle node): uart_(false) @@ -120,29 +131,40 @@ Bno055Node::Bno055Node(ros::NodeHandle node): uart_(false) if(bno055_set_gyro_unit(BNO055_GYRO_UNIT_RPS) != BNO055_SUCCESS) ROS_ERROR_STREAM("Error setting gyroscope unit to rad/s."); + + if(bno055_set_euler_unit(BNO055_EULER_UNIT_RAD) != BNO055_SUCCESS) + ROS_ERROR_STREAM("Error setting Euler angles unit to rad."); if(bno055_set_temp_unit(BNO055_TEMP_UNIT_CELSIUS) != BNO055_SUCCESS) ROS_ERROR_STREAM("Error setting temperature unit to Celsius."); - + if(bno055_set_data_output_format(0x01) != BNO055_SUCCESS) ROS_ERROR_STREAM("Error setting data output format."); if(bno055_set_operation_mode(mode) != BNO055_SUCCESS) ROS_ERROR_STREAM("Error setting operation mode."); + + nh.param("frame_id",frameId_,std::string("imu_link")); + nh.param("linear_acceleration_stddev",accStdDev_,0.0); + nh.param("angular_velocity_stddev",gyroStdDev_,0.0); + nh.param("magnetic_field_stddev",magStdDev_,0.0); + nh.param("orientation_stddev",orientationStdDev_,0.0); - imuPub_=node_.advertise("imu",100); + dataRawPub_=node_.advertise("data_raw",100); + dataPub_=node_.advertise("data",100); temperaturePub_=node_.advertise("temperature",100); - magneticFieldPub_=node_.advertise("magnetic_field",100); - accelPub_=node_.advertise("acceleration",100); + magneticFieldPub_=node_.advertise("mag",100); + rpyPub_=node_.advertise("rpy",100); gravityPub_=node_.advertise("gravity",100); } Bno055Node::~Bno055Node(void) { - imuPub_.shutdown(); + dataRawPub_.shutdown(); + dataPub_.shutdown(); temperaturePub_.shutdown(); magneticFieldPub_.shutdown(); - accelPub_.shutdown(); + rpyPub_.shutdown(); gravityPub_.shutdown(); if(uart_) BNO055_tty_close(&bno055_); @@ -153,62 +175,72 @@ void Bno055Node::publish(void) const { ros::Time time=ros::Time::now(); - sensor_msgs::Imu imuMsg; - imuMsg.header.stamp=time; - imuMsg.header.frame_id="imu"; + sensor_msgs::Imu dataMsg; + dataMsg.header.stamp=time; + dataMsg.header.frame_id=frameId_; - tf::quaternionEigenToMsg(quaternion_,imuMsg.orientation); - for (int i=0; i < 9; i++) imuMsg.orientation_covariance[i]=0.0; - imuMsg.orientation_covariance[0]=0.0; - imuMsg.orientation_covariance[3]=0.0; - imuMsg.orientation_covariance[6]=0.0; - - tf::vectorEigenToMsg(gyro_,imuMsg.angular_velocity); - for (int i=0; i < 9; i++) imuMsg.angular_velocity_covariance[i]=0.0; - imuMsg.angular_velocity_covariance[0]=0.0; - imuMsg.angular_velocity_covariance[3]=0.0; - imuMsg.angular_velocity_covariance[6]=0.0; - - tf::vectorEigenToMsg(linearAccel_,imuMsg.linear_acceleration); - for (int i=0; i < 9; i++) imuMsg.linear_acceleration_covariance[i]=0.0; - imuMsg.linear_acceleration_covariance[0]=0.0; - imuMsg.linear_acceleration_covariance[3]=0.0; - imuMsg.linear_acceleration_covariance[6]=0.0; - - imuPub_.publish(imuMsg); + for (int i=0; i < 9; i++) dataMsg.orientation_covariance[i]=-1.0; + + tf::vectorEigenToMsg(gyro_,dataMsg.angular_velocity); + for (int i=0; i < 9; i++) dataMsg.angular_velocity_covariance[i]=0.0; + dataMsg.angular_velocity_covariance[0]=sqr(gyroStdDev_); + dataMsg.angular_velocity_covariance[3]=sqr(gyroStdDev_); + dataMsg.angular_velocity_covariance[6]=sqr(gyroStdDev_); + + tf::vectorEigenToMsg(accel_,dataMsg.linear_acceleration); + for (int i=0; i < 9; i++) dataMsg.linear_acceleration_covariance[i]=0.0; + dataMsg.linear_acceleration_covariance[0]=sqr(accStdDev_); + dataMsg.linear_acceleration_covariance[3]=sqr(accStdDev_); + dataMsg.linear_acceleration_covariance[6]=sqr(accStdDev_); + + dataRawPub_.publish(dataMsg); + tf::quaternionEigenToMsg(quaternion_,dataMsg.orientation); + for (int i=0; i < 9; i++) dataMsg.orientation_covariance[i]=0.0; + dataMsg.orientation_covariance[0]=sqr(orientationStdDev_); + dataMsg.orientation_covariance[3]=sqr(orientationStdDev_); + dataMsg.orientation_covariance[6]=sqr(orientationStdDev_); + + tf::vectorEigenToMsg(linearAccel_,dataMsg.linear_acceleration); + for (int i=0; i < 9; i++) dataMsg.linear_acceleration_covariance[i]=0.0; + dataMsg.linear_acceleration_covariance[0]=sqr(accStdDev_); + dataMsg.linear_acceleration_covariance[3]=sqr(accStdDev_); + dataMsg.linear_acceleration_covariance[6]=sqr(accStdDev_); + + dataPub_.publish(dataMsg); + sensor_msgs::Temperature temperatureMsg; temperatureMsg.header.stamp=time; - temperatureMsg.header.frame_id="imu"; + temperatureMsg.header.frame_id=frameId_; temperatureMsg.temperature=temp_; temperatureMsg.variance=0.0; - + temperaturePub_.publish(temperatureMsg); sensor_msgs::MagneticField magneticFieldMsg; magneticFieldMsg.header.stamp=time; - magneticFieldMsg.header.frame_id="imu"; + magneticFieldMsg.header.frame_id=frameId_; tf::vectorEigenToMsg(mag_,magneticFieldMsg.magnetic_field); for (int i=0; i < 9; i++) magneticFieldMsg.magnetic_field_covariance[i]=0.0; - magneticFieldMsg.magnetic_field_covariance[0]=0.0; - magneticFieldMsg.magnetic_field_covariance[3]=0.0; - magneticFieldMsg.magnetic_field_covariance[6]=0.0; + magneticFieldMsg.magnetic_field_covariance[0]=sqr(magStdDev_); + magneticFieldMsg.magnetic_field_covariance[3]=sqr(magStdDev_); + magneticFieldMsg.magnetic_field_covariance[6]=sqr(magStdDev_); magneticFieldPub_.publish(magneticFieldMsg); - geometry_msgs::Vector3Stamped accelMsg; - accelMsg.header.stamp=time; - accelMsg.header.frame_id="imu"; + geometry_msgs::Vector3Stamped rpyMsg; + rpyMsg.header.stamp=time; + rpyMsg.header.frame_id=frameId_; - tf::vectorEigenToMsg(accel_,accelMsg.vector); + tf::vectorEigenToMsg(rpy_,rpyMsg.vector); - accelPub_.publish(accelMsg); + rpyPub_.publish(rpyMsg); geometry_msgs::Vector3Stamped gravityMsg; gravityMsg.header.stamp=time; - gravityMsg.header.frame_id="imu"; + gravityMsg.header.frame_id=frameId_; tf::vectorEigenToMsg(accel_,gravityMsg.vector); @@ -239,6 +271,13 @@ void Bno055Node::sample(void) gyro.z/BNO055_GYRO_DIV_RPS; else ROS_ERROR_STREAM("Error reading gyroscope."); + struct bno055_euler_t euler; + if(bno055_read_euler_hrp(&euler) == BNO055_SUCCESS) + rpy_ << euler.r/BNO055_EULER_DIV_RAD, + euler.p/BNO055_EULER_DIV_RAD, + euler.h/BNO055_EULER_DIV_RAD; + else ROS_ERROR_STREAM("Error reading Euler angles.\n"); + s8 temp; if(bno055_read_temp_data(&temp) == BNO055_SUCCESS) temp_=temp/BNO055_TEMP_DIV_CELSIUS; @@ -272,11 +311,11 @@ void Bno055Node::sample(void) int main(int argc,char* argv[]) { ros::init(argc,argv,"bno055_node"); - ros::NodeHandle node; + ros::NodeHandle node("imu"); Bno055Node bno055Node(node); - ros::Rate loop(1000); + ros::Rate loop(100); while(ros::ok()) { bno055Node.sample();