From 2a5297cbb46912f43c9408add0e9b1ec46eab38a Mon Sep 17 00:00:00 2001 From: yikestone Date: Mon, 1 Apr 2019 02:08:03 +0530 Subject: [PATCH] Added gazebo world and model. Created launch files. gmapping not working. --- CMakeLists.txt | 3 + gazebo_sim/worlds/playground.world | 2045 +++++++++++++++++++ launch/gazebo_sim.launch | 11 + launch/gazebo_sim/depth_to_laser_sim.launch | 8 + launch/gazebo_sim/gmapping_sim.launch | 9 + launch/gazebo_sim/static_tf_sim.launch | 3 + launch/gazebo_sim/teleop.launch | 4 + src/evp-encrypt.cxx | 161 -- 8 files changed, 2083 insertions(+), 161 deletions(-) create mode 100644 gazebo_sim/worlds/playground.world create mode 100644 launch/gazebo_sim.launch create mode 100644 launch/gazebo_sim/depth_to_laser_sim.launch create mode 100644 launch/gazebo_sim/gmapping_sim.launch create mode 100644 launch/gazebo_sim/static_tf_sim.launch create mode 100644 launch/gazebo_sim/teleop.launch delete mode 100644 src/evp-encrypt.cxx diff --git a/CMakeLists.txt b/CMakeLists.txt index ecb1a64..dd7dc4a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(qt_pi) + +set(CMAKE_CXX_FLAGS "-fpermissive -std=c++0x") + find_package(catkin REQUIRED) catkin_python_setup() diff --git a/gazebo_sim/worlds/playground.world b/gazebo_sim/worlds/playground.world new file mode 100644 index 0000000..4b73588 --- /dev/null +++ b/gazebo_sim/worlds/playground.world @@ -0,0 +1,2045 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 0.193814 -0.057177 0 0 -0 0 + + + + + 26.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 26.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0.007886 -3.91706 0 0 -0 0 + 0 + 0 + 0 + + + + + + 8 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 8 0.15 2.5 + + + + + 1 1 1 1 + + + 13.3079 0.007941 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 26.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 26.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0.007886 3.93294 0 0 -0 3.14159 + 0 + 0 + 0 + + + -9.18476 0.019386 0 0 0 -1.5708 + + -3.12221 0 1.25 0 -0 0 + + + 1.75558 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 1.75558 0.15 2.5 + + + -3.12221 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.438191 0 1.25 0 -0 0 + + + 3.56521 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.56521 0.15 2.5 + + + 0.438191 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 3.5604 0 1.25 0 -0 0 + + + 0.879203 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.879203 0.15 2.5 + + + 3.5604 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + -1.79442 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + -1.79442 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.6708 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 2.6708 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + + + + 4.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4.25 0.15 2.5 + + + + + 1 1 1 1 + + + -11.2579 -1.35252 0 0 -0 3.14159 + 0 + 0 + 0 + + + -6.89009 -0.019386 0 0 0 -1.5708 + + -2.05546 0 1.25 0 -0 0 + + + 3.88908 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.88908 0.15 2.5 + + + -2.05546 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.39454 0 1.25 0 -0 0 + + + 3.21092 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.21092 0.15 2.5 + + + 2.39454 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.339077 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.339077 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + -2.84009 1.17017 0 0 -0 0 + + -2.27097 0 1.25 0 -0 0 + + + 3.70805 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.70805 0.15 2.5 + + + -2.27097 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.30403 0 1.25 0 -0 0 + + + 3.64195 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.64195 0.15 2.5 + + + 2.30403 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.033055 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.033055 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + 1.21568 0.006434 0 0 -0 1.5708 + + -2.30688 0 1.25 0 -0 0 + + + 3.38624 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.38624 0.15 2.5 + + + -2.30688 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.14312 0 1.25 0 -0 0 + + + 3.71376 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.71376 0.15 2.5 + + + 2.14312 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + -0.163762 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + -0.163762 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + + + + 5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 5 0.15 2.5 + + + + + 1 1 1 1 + + + 3.62057 -1.58539 0 0 -0 0 + 0 + 0 + 0 + + + 6.04557 -2.76039 0 0 0 -1.5708 + + -0.84415 0 1.25 0 -0 0 + + + 0.811701 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.811701 0.15 2.5 + + + -0.84415 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.85585 0 1.25 0 -0 0 + + + 0.788299 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.788299 0.15 2.5 + + + 0.85585 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.011701 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.011701 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + 3.40806 1.10972 0 0 -0 1.5708 + + -2.20336 0 1.25 0 -0 0 + + + 1.09329 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 1.09329 0.15 2.5 + + + -2.20336 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.181127 0 1.25 0 -0 0 + + + 1.87567 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 1.87567 0.15 2.5 + + + 0.181127 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.38448 0 1.25 0 -0 0 + + + 0.731036 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.731036 0.15 2.5 + + + 2.38448 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + -1.20671 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + -1.20671 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 1.56896 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 1.56896 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + + + + 4.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4.5 0.15 2.5 + + + + + 1 1 1 1 + + + 5.60318 1.67301 0 0 -0 0 + 0 + 0 + 0 + + + 7.77818 2.72301 0 0 -0 1.5708 + + -0.729206 0 1.25 0 -0 0 + + + 0.791587 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.791587 0.15 2.5 + + + -0.729206 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.845794 0 1.25 0 -0 0 + + + 0.558413 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.558413 0.15 2.5 + + + 0.845794 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.116587 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.116587 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + 10.5278 0.085507 0 0 0 -0.523599 + + -0.945166 0 1.25 0 -0 0 + + + 4.60967 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 4.60967 0.15 2.5 + + + -0.945166 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.75483 0 1.25 0 -0 0 + + + 0.990333 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.990333 0.15 2.5 + + + 2.75483 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 1.80967 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 1.80967 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + 10.3472 -1.82517 0 0 0 -1.5708 + + -0.985004 0 1.25 0 -0 0 + + + 2.27999 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 2.27999 0.15 2.5 + + + -0.985004 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 1.59 0 1.25 0 -0 0 + + + 1.07001 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 1.07001 0.15 2.5 + + + 1.59 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.604992 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.604992 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + + + + 8 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 8 0.15 2.5 + + + + + 1 1 1 1 + + + -13.2921 0.007941 0 0 0 -1.5708 + 0 + 0 + 0 + + 1 + + + + 0 0 0.1 0 -0 0 + + 20 + 0.1 0 -0.05 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + + 0.4 0.2 0.1 + + + 10 + + + + + + + + + + + + + + + + + 0.4 0.2 0.1 + + + + + 0.15 0 -0.05 0 -0 0 + + + 0.05 + + + + + 0 + 0.01 + + + + 0 + 0.001 + + + + + 0 + 0 + 1 + 1 + + + + + + + 10 + + + 0.15 0 -0.05 0 -0 0 + + + 0.05 + + + + 0 + 0 + 0 + + + -0.1 0.2 0.1 0 -0 0 + + 5 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + + + 10 + 10 + + + + + + + 0 + 0.01 + + + + 0 + 0.001 + + + + + + 0.1 + + + 10 + + + + + 0.1 + + + + 0 + 0 + 0 + + + -0.1 -0.2 0.1 0 -0 0 + + 5 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + + + 10 + 10 + + + + + + + 0 + 0.01 + + + + 0 + 0.001 + + + + + + 0.1 + + + 10 + + + + + 0.1 + + + + 0 + 0 + 0 + + + 0 0 0 0 -0 0 + left_wheel + chassis + + 0 1 0 + 0 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + right_wheel + chassis + + 0 1 0 + 0 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + + 0.1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + + 0.073 0.276 0.072 + + + 10 + + + + + + + + + + + + + + + + + model://kinect/meshes/kinect.dae + + + + + 15 + + 1.2215 + + 640 + 480 + R8G8B8 + + + 0.05 + 100 + + + + qt_pi + 0.2 + 1 + 15 + camera_ir + /qt_pi/camera_ir/color/image_raw + /qt_pi/camera_ir/color/camera_info + /qt_pi/camera_ir/depth/image_raw + /camera_ir/depth/camera_info + /qt_pi/camera_ir/depth/points + camera_link + 0.05 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + + 0.2 0 0.2 0 -0 0 + 0 + 0 + 0 + + + kinect::link + chassis + + + 1 + 30 + left_wheel_hinge + qt_pi + right_wheel_hinge + 0.4 + 0.2 + 0 + 5 + cmd_vel + odom + base_link + odom + 1 + 1 + 1 + 0 + 0 + + 2.20517 -4.60502 0 0 -0 0 + + + 47 5000000 + 47 576261785 + 1554061633 217805104 + 47005 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0.007886 -3.91706 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 13.3079 0.007941 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.007886 3.93294 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -9.18476 0.019386 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -11.2579 -1.35252 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89009 -0.019386 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.84009 1.17017 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.21568 0.006434 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.62057 -1.58539 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.04557 -2.76039 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.40806 1.10972 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.60318 1.67301 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.77818 2.72301 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.5278 0.085507 0 0 0 -0.523599 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3472 -1.82517 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2921 0.007941 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 0 0 + 1 1 1 + + 0 0 0.1 0 0 0 + 0 0 0 0 0 0 + 0 0 0 0 0 0 + 0 0 0 0 0 0 + + + 0.2 0 0.2 0 0 0 + 0 -0 0 0 0 0 + -0 0 0 0 0 0 + -0 0 0 0 0 0 + + + -0.1 0.2 0.1 0 0 0 + 0 -0 0 0 0 0 + 0 0 0 0 0 0 + 0 0 0 0 0 0 + + + -0.1 -0.2 0.1 0 0 0 + -0 0 0 0 0 0 + 0 0 0 0 0 0 + -0 0 0 0 0 0 + + + + 0 0 10 0 0 0 + + + + + 6.80207 -4.04261 25.2995 -4.6e-05 1.5698 1.10815 + orbit + perspective + + + + \ No newline at end of file diff --git a/launch/gazebo_sim.launch b/launch/gazebo_sim.launch new file mode 100644 index 0000000..b5b59b8 --- /dev/null +++ b/launch/gazebo_sim.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/launch/gazebo_sim/depth_to_laser_sim.launch b/launch/gazebo_sim/depth_to_laser_sim.launch new file mode 100644 index 0000000..e815096 --- /dev/null +++ b/launch/gazebo_sim/depth_to_laser_sim.launch @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/launch/gazebo_sim/gmapping_sim.launch b/launch/gazebo_sim/gmapping_sim.launch new file mode 100644 index 0000000..bdb3aee --- /dev/null +++ b/launch/gazebo_sim/gmapping_sim.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/launch/gazebo_sim/static_tf_sim.launch b/launch/gazebo_sim/static_tf_sim.launch new file mode 100644 index 0000000..e350868 --- /dev/null +++ b/launch/gazebo_sim/static_tf_sim.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/launch/gazebo_sim/teleop.launch b/launch/gazebo_sim/teleop.launch new file mode 100644 index 0000000..58d8b2f --- /dev/null +++ b/launch/gazebo_sim/teleop.launch @@ -0,0 +1,4 @@ + + + + diff --git a/src/evp-encrypt.cxx b/src/evp-encrypt.cxx deleted file mode 100644 index c60c6a6..0000000 --- a/src/evp-encrypt.cxx +++ /dev/null @@ -1,161 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include - -static const unsigned int KEY_SIZE = 32; -static const unsigned int BLOCK_SIZE = 16; - -template -struct zallocator -{ -public: - typedef T value_type; - typedef value_type* pointer; - typedef const value_type* const_pointer; - typedef value_type& reference; - typedef const value_type& const_reference; - typedef std::size_t size_type; - typedef std::ptrdiff_t difference_type; - - pointer address (reference v) const {return &v;} - const_pointer address (const_reference v) const {return &v;} - - pointer allocate (size_type n, const void* hint = 0) { - if (n > std::numeric_limits::max() / sizeof(T)) - throw std::bad_alloc(); - return static_cast (::operator new (n * sizeof (value_type))); - } - - void deallocate(pointer p, size_type n) { - OPENSSL_cleanse(p, n*sizeof(T)); - ::operator delete(p); - } - - size_type max_size() const { - return std::numeric_limits::max() / sizeof (T); - } - - template - struct rebind - { - typedef zallocator other; - }; - - void construct (pointer ptr, const T& val) { - new (static_cast(ptr) ) T (val); - } - - void destroy(pointer ptr) { - static_cast(ptr)->~T(); - } - -#if __cpluplus >= 201103L - template - void construct (U* ptr, Args&& ... args) { - ::new (static_cast (ptr) ) U (std::forward (args)...); - } - - template - void destroy(U* ptr) { - ptr->~U(); - } -#endif -}; - -typedef unsigned char byte; -typedef std::basic_string, zallocator > secure_string; -using EVP_CIPHER_CTX_free_ptr = std::unique_ptr; - -void gen_params(byte key[KEY_SIZE], byte iv[BLOCK_SIZE]); -void aes_encrypt(const byte key[KEY_SIZE], const byte iv[BLOCK_SIZE], const secure_string& ptext, secure_string& ctext); -void aes_decrypt(const byte key[KEY_SIZE], const byte iv[BLOCK_SIZE], const secure_string& ctext, secure_string& rtext); - -// g++ -Wall -std=c++11 evp-encrypt.cxx -o evp-encrypt.exe -lcrypto -int main(int argc, char* argv[]) -{ - // Load the necessary cipher - EVP_add_cipher(EVP_aes_256_cbc()); - - // plaintext, ciphertext, recovered text - secure_string ptext = "Now is the time for all good men to come to the aide of their country"; - secure_string ctext, rtext; - - byte key[KEY_SIZE], iv[BLOCK_SIZE]; - gen_params(key, iv); - - aes_encrypt(key, iv, ptext, ctext); - aes_decrypt(key, iv, ctext, rtext); - - OPENSSL_cleanse(key, KEY_SIZE); - OPENSSL_cleanse(iv, BLOCK_SIZE); - - std::cout << "Original message:\n" << ptext << std::endl; - std::cout << "Recovered message:\n" << rtext << std::endl; - - return 0; -} - -void gen_params(byte key[KEY_SIZE], byte iv[BLOCK_SIZE]) -{ - int rc = RAND_bytes(key, KEY_SIZE); - if (rc != 1) - throw std::runtime_error("RAND_bytes key failed"); - - rc = RAND_bytes(iv, BLOCK_SIZE); - if (rc != 1) - throw std::runtime_error("RAND_bytes for iv failed"); -} - -void aes_encrypt(const byte key[KEY_SIZE], const byte iv[BLOCK_SIZE], const secure_string& ptext, secure_string& ctext) -{ - EVP_CIPHER_CTX_free_ptr ctx(EVP_CIPHER_CTX_new(), ::EVP_CIPHER_CTX_free); - int rc = EVP_EncryptInit_ex(ctx.get(), EVP_aes_256_cbc(), NULL, key, iv); - if (rc != 1) - throw std::runtime_error("EVP_EncryptInit_ex failed"); - - // Recovered text expands upto BLOCK_SIZE - ctext.resize(ptext.size()+BLOCK_SIZE); - int out_len1 = (int)ctext.size(); - - rc = EVP_EncryptUpdate(ctx.get(), (byte*)&ctext[0], &out_len1, (const byte*)&ptext[0], (int)ptext.size()); - if (rc != 1) - throw std::runtime_error("EVP_EncryptUpdate failed"); - - int out_len2 = (int)ctext.size() - out_len1; - rc = EVP_EncryptFinal_ex(ctx.get(), (byte*)&ctext[0]+out_len1, &out_len2); - if (rc != 1) - throw std::runtime_error("EVP_EncryptFinal_ex failed"); - - // Set cipher text size now that we know it - ctext.resize(out_len1 + out_len2); -} - -void aes_decrypt(const byte key[KEY_SIZE], const byte iv[BLOCK_SIZE], const secure_string& ctext, secure_string& rtext) -{ - EVP_CIPHER_CTX_free_ptr ctx(EVP_CIPHER_CTX_new(), ::EVP_CIPHER_CTX_free); - int rc = EVP_DecryptInit_ex(ctx.get(), EVP_aes_256_cbc(), NULL, key, iv); - if (rc != 1) - throw std::runtime_error("EVP_DecryptInit_ex failed"); - - // Recovered text contracts upto BLOCK_SIZE - rtext.resize(ctext.size()); - int out_len1 = (int)rtext.size(); - - rc = EVP_DecryptUpdate(ctx.get(), (byte*)&rtext[0], &out_len1, (const byte*)&ctext[0], (int)ctext.size()); - if (rc != 1) - throw std::runtime_error("EVP_DecryptUpdate failed"); - - int out_len2 = (int)rtext.size() - out_len1; - rc = EVP_DecryptFinal_ex(ctx.get(), (byte*)&rtext[0]+out_len1, &out_len2); - if (rc != 1) - throw std::runtime_error("EVP_DecryptFinal_ex failed"); - - // Set recovered text size now that we know it - rtext.resize(out_len1 + out_len2); -} -