diff --git a/README.md b/README.md index f4c9d9d..92ede0e 100644 --- a/README.md +++ b/README.md @@ -9,14 +9,17 @@ Install and Configure ROS Package --------------------------------- 1) Install dependencies: - $ sudo apt-get install python-visual + ``$ sudo apt-get install python-visual`` 2) Download code: - $ cd ~/catkin_workspace/src - $ git clone https://github.com/KristofRobot/razor_imu_9dof.git - $ cd .. - $ catkin_make + ``$ cd ~/catkin_workspace/src`` + + ``$ git clone https://github.com/blmhemu/razor_imu_9dof.git`` + + ``$ cd ..`` + + ``$ catkin_make`` Install Arduino firmware @@ -76,6 +79,26 @@ Publisher only with diagnostics: $ roslaunch razor_imu_9dof razor-display.launch +Conventions +----------- + +The IMU follows ENU Convention. + +East = 0 degree + +North = 90 degree + +West = 180 degree / -180 degree + +South = -90 degree + +Accelerations and angular accelerations are also taken care of. (Right Hand Rule) + +This standard is in lines with ROS REP 105. This driver can be used for direct interfacing with robot_localization node. + +The setup used is shown below for reference + +![alt text](https://raw.githubusercontent.com/blmhemu/razor_imu_9dof/indigo-devel/razor.png) Calibrate --------- diff --git a/nodes/display_3D_visualization.py b/nodes/display_3D_visualization.py index a681216..38fa36b 100755 --- a/nodes/display_3D_visualization.py +++ b/nodes/display_3D_visualization.py @@ -73,14 +73,14 @@ def shutdown_hook(): label(pos=(-0.5,0.6,0),text="Roll (degrees / radians)",box=0,opacity=0) label(pos=(0.5,0.6,0),text="Pitch (degrees / radians)",box=0,opacity=0) label(pos=(0.0,0.02,0),text="Yaw (degrees / radians)",box=0,opacity=0) -label(pos=(0.0,-0.16,0),text="N",box=0,opacity=0,color=color.yellow) -label(pos=(0.0,-0.64,0),text="S",box=0,opacity=0,color=color.yellow) -label(pos=(-0.24,-0.4,0),text="W",box=0,opacity=0,color=color.yellow) -label(pos=(0.24,-0.4,0),text="E",box=0,opacity=0,color=color.yellow) -label(pos=(0.18,-0.22,0),height=7,text="NE",box=0,color=color.yellow) -label(pos=(-0.18,-0.22,0),height=7,text="NW",box=0,color=color.yellow) -label(pos=(0.18,-0.58,0),height=7,text="SE",box=0,color=color.yellow) -label(pos=(-0.18,-0.58,0),height=7,text="SW",box=0,color=color.yellow) +label(pos=(0.0,-0.16,0),text="E",box=0,opacity=0,color=color.yellow) +label(pos=(0.0,-0.64,0),text="W",box=0,opacity=0,color=color.yellow) +label(pos=(-0.24,-0.4,0),text="N",box=0,opacity=0,color=color.yellow) +label(pos=(0.24,-0.4,0),text="S",box=0,opacity=0,color=color.yellow) +label(pos=(0.18,-0.22,0),height=7,text="SE",box=0,color=color.yellow) +label(pos=(-0.18,-0.22,0),height=7,text="NE",box=0,color=color.yellow) +label(pos=(0.18,-0.58,0),height=7,text="SW",box=0,color=color.yellow) +label(pos=(-0.18,-0.58,0),height=7,text="NW",box=0,color=color.yellow) rollLabel = label(pos=(-0.5,0.52,0),text="-",box=0,opacity=0,height=12) pitchLabel = label(pos=(0.5,0.52,0),text="-",box=0,opacity=0,height=12) @@ -94,7 +94,7 @@ def shutdown_hook(): # Main scene objects scene.select() -# Reference axis (x,y,z) - using ROS conventions (REP 103) - z is up, y left (west, 90 deg), x is forward (north, 0 deg) +# Reference axis (x,y,z) - using ROS conventions (REP 103) - z is up, y left (north, 90 deg), x is forward (east, 0 deg) # In visual, z runs up, x runs forward, y runs left (see scene.up command earlier) # So everything is good arrow(color=color.green,axis=(1,0,0), shaftwidth=0.04, fixedwidth=1) diff --git a/nodes/imu_node.py b/nodes/imu_node.py index 27a4275..d8f47a2 100755 --- a/nodes/imu_node.py +++ b/nodes/imu_node.py @@ -225,7 +225,8 @@ def reconfig_callback(config, level): words = string.split(line,",") # Fields split if len(words) > 2: #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) - yaw_deg = -float(words[0]) + #Added -90 to make it compatible with ENU (REP 103) + yaw_deg = -90-float(words[0]) yaw_deg = yaw_deg + imu_yaw_calibration if yaw_deg > 180.0: yaw_deg = yaw_deg - 360.0 @@ -233,8 +234,8 @@ def reconfig_callback(config, level): yaw_deg = yaw_deg + 360.0 yaw = yaw_deg*degrees2rad #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103) - pitch = -float(words[1])*degrees2rad - roll = float(words[2])*degrees2rad + pitch = float(words[1])*degrees2rad + roll = -float(words[2])*degrees2rad # Publish message # AHRS firmware accelerations are negated @@ -243,9 +244,9 @@ def reconfig_callback(config, level): imuMsg.linear_acceleration.y = float(words[4]) * accel_factor imuMsg.linear_acceleration.z = float(words[5]) * accel_factor - imuMsg.angular_velocity.x = float(words[6]) - #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103) - imuMsg.angular_velocity.y = -float(words[7]) + #in AHRS firmware x axis points right, in ROS x axis points left (see REP 103) + imuMsg.angular_velocity.x = -float(words[6]) + imuMsg.angular_velocity.y = float(words[7]) #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) imuMsg.angular_velocity.z = -float(words[8]) diff --git a/razor.png b/razor.png new file mode 100644 index 0000000..1b79261 Binary files /dev/null and b/razor.png differ