---
slideOptions:
transition: slide
---
<style>
.reveal {
font-size: 24px;
}
</style>
# CODE Template
"templates of functions to copy and paste"
For better look check this note, please visit [HackMD.io](https://hackmd.io/@libernormous/code_temp)
by:
[Liber Normous](https://hackmd.io/@libernormous)
---
# MATLAB showing occupancy grid map
```c=
figure(212);
mapimg = imread('/home/zee/catkin_ws/src/turtlebot3/turtlebot3_navigation/maps/map_house_turtlebot.pgm');
imageNorm = double(mapimg)/255;
imageOccupancy = 1 - imageNorm;
map = occupancyMap(imageOccupancy,20);
show(map)
hold on;
```
# MATLAB figure template
```c=
%% ----------Plot x vs y---------------
figure(4);
plot(simout.xa, simout.ya, simout.G_xt, simout.G_yt, '*', xa_init, ya_init, 'g*', 'LineWidth', 2, 'MarkerSize', 4), xlabel('x(m)'), ylabel('y(m)'), axis equal, grid on, hold on;
title(['robot path']);
legend({'robot path', 'goal position', 'start position'},'Location','northeast')
set(gca,'FontSize',12);
set(gca,'FontName','serif');
set(gca,'FontWeight','bold');
set(gca,'LineWidth',2);
set(gcf, 'Renderer', 'painters');
```
```c=
title('Particles plot', 'interpreter', 'latex');
legend({'Particles', 'MCL prediction', 'Ground truth'},'Location','southeast', 'interpreter', 'latex')
xlabel('X (m)', 'interpreter', 'latex' );
ylabel('Y (m)', 'interpreter', 'latex' );
set(gca,'FontSize',12);
set(gca,'FontName','serif');
set(gca,'FontWeight','bold');
set(gca,'LineWidth',2);
set(gcf, 'Renderer', 'painters');
```
```c=
%% -----------Plot actuator output-------------
figure(5);
%subplot 1
subplot(2,1,1),plot(simout.tout, simout.poser, 'LineWidth', 2), xlabel('t(s)'), ylabel('position error (m)'), grid on, hold on;
set(gca,'FontSize',12); %2
set(gca,'FontName','serif');
set(gca,'FontWeight','bold');
set(gca,'LineWidth',2);
set(gcf, 'Renderer', 'painters');
%subplot 2
subplot(2,1,2),plot(simout.tout, simout.orrer, 'LineWidth', 2), xlabel('t(s)'), ylabel('heading error (deg)'), grid on, hold on;%, axis equal;
set(gca,'FontSize',12); %
set(gca,'FontName','serif');
set(gca,'FontWeight','bold');
set(gca,'LineWidth',2);
set(gcf, 'Renderer', 'painters');
```
---
# MATLAB 3D plot
```c=
figure(5506)
f1 = @(x,y) surf_magnitude_1(x, y);
fsurf(f1,[0.25 0.45 -1 1]);
xlabel('$d_{obs}$ (m)', 'interpreter', 'latex' );
ylabel('$\theta_{obs}$ ($\pi$ rad)', 'interpreter', 'latex' );
zlabel('$V_{dm}$', 'interpreter', 'latex' );
camlight;
box on;
set(gca,'FontSize',14);
set(gca,'FontName','serif');
set(gca,'LineWidth',1.5);
set(gcf, 'Renderer', 'painters');
```
---
# MATLAB MAV filter
```c=
AVG_window = 20;
AVG_coef = ones(1, AVG_window)/AVG_window;
vr_avg = filter(AVG_coef, 1, vr);
vl_avg = filter(AVG_coef, 1, vl);
```
---
# MATLAB CLASS
```c=
classdef robot < handle
% `handle is important so youi can values inside properties`
properties(Access = public)
end
properties(Access = private)
odom_msg
vel_pub
vel_msg
odom_sub
amcl_sub
end
methods(Access = public)
function obj = robot()
% CONSTRUCTOR
% Codes in this function is run once at the begining
try
rosinit;
catch
disp('already started');
end
%% pub init
obj.vel_pub = rospublisher('/cmd_vel','geometry_msgs/Twist');
pause(2); % Wait to ensure publisher is registered
obj.vel_msg = rosmessage(obj.vel_pub); % pub message
%% sub init
obj.odom_msg = rosmessage('nav_msgs/Odometry');
obj.odom_sub = rossubscriber('/odom',@obj.odom_cb);
obj.amcl_sub = rossubscriber('/amcl_pose');
end
function disp_odom(obj)
%this function take variables from `properties` and not
%modifying them that is why it does not need return `obj`
disp(obj.odom_msg.Pose.Pose.Position)
end
end
methods(Access = private)
function obj = odom_cb(obj,~, message)
%function obj = odom_cb(obj,src, message)
%if you wish to use variables inside `properties` add `obj` as
%as arguments. and if you wish to use object from `rossubscriber`
%`use` src as arguments
obj.odom_msg = message;
end
end
end
```
---
# MATLAB symbolic derivation
```c=
clear;
clc;
syms r(t) theta(t);
f=r*exp(j*theta);
f_dot = diff(f,t)
f_ddot = diff(f_dot,t)
% subs(f_ddot,vu_dot)
% eq1 = exp(theta(t)*1i)*diff(r(t), t, t) - exp(theta(t)*1i)*r(t)*diff(theta(t), t)^2 == au;
% eq2 = exp(theta(t)*1i)*r(t)*diff(theta(t), t, t)*1i + exp(theta(t)*1i)*diff(r(t), t)*diff(theta(t), t)*2i == aw;
vu = exp(theta(t)*1i)*diff(r(t), t);
vw = 1i*exp(theta(t)*1i)*r(t)*diff(theta(t), t);
subs(f_ddot,vu)
```
---
# Matlab finding Rise Time, Settling Time
Source : https://www.mathworks.com/help/control/ref/lti.stepinfo.html#d123e129937
Prepare time series data then use this code:
```c=
S = stepinfo(simout.poser,simout.tout)
```
---
# MATLAB finding index and partial plot
Plotting only $50<t<90$
```c=
t_90 = find(simout.tout<90 & simout.tout>50)
figure(20)
plot(simout.tout(t_90), simout.poser(t_90))
```
---
# MATLAB analyzing settling time magnitude
```c=
S = stepinfo(simout.poser,simout.tout)
t_90 = find(simout.tout<90 & simout.tout>50)
figure(20);
plot(simout.tout(t_90), simout.poser(t_90));
disp(['std dev = ', num2str(std(simout.poser(t_90)))]);
disp(['average = ', num2str(mean(simout.poser(t_90)))]);
```
---
# MATLAB Choosing random number
Random integer choosing. Sample 2 random number from 4 number.
```c=
randi(4,[2,1])
```
# PYTHON-ROS saving `*.json` files
```python=
#!/home/zee/Manually_Installed/anaconda3/envs/ros1/bin/python
from numpy.core.fromnumeric import size
import rospy
import tf
import string
import math
import time
import sys
import json
import os
from geometry_msgs.msg import PoseArray
from geometry_msgs.msg import PoseWithCovarianceStamped
from gazebo_msgs.msg import ModelStates
from tf.transformations import euler_from_quaternion, quaternion_from_euler
# saving and working directory
os.chdir('/media/zee/53FC06C50A4891B9/Project/m-47-smflc_QE/m-47-smflc_QE_code')
class Robot :
def __init__(self):
#particles related variable
self.particlesub = rospy.Subscriber('/particlecloud', PoseArray, self.cb_particle, queue_size=10)
self.particlesmsg = PoseArray
self.ptcl_tot_data = {
"data" : [] #array element is iteration
}
#amcl related variable
self.amclsub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.cb_amcl, queue_size=10)
self.amclmsg = PoseWithCovarianceStamped
self.amcl_tot_data = {
"data" : [] #array element is iteration
}
#gazebo robot states related variable
self.gazebostatessub = rospy.Subscriber('/gazebo/model_states', ModelStates, self.cb_gazebostates, queue_size=10)
self.gazebostatesmsg = ModelStates
self.gazebostates_tot_data = {
"data" : [] #array element is iteration
}
print('init done')
def unregister_all_subscriber(self):
self.particlesub.unregister()
self.amclsub.unregister()
self.gazebostatessub.unregister()
def cb_gazebostates(self, data):
self.gazebostatesmsg = data
neuronbot2_x = data.pose[2].position.x
neuronbot2_y = data.pose[2].position.y
x_or = data.pose[2].orientation.x
y_or = data.pose[2].orientation.y
z_or = data.pose[2].orientation.z
w_or = data.pose[2].orientation.w
orientation_list = [x_or, y_or, z_or, w_or]
(roll_or, pitch_or, yaw_or) = euler_from_quaternion (orientation_list)
neuronbot2_t = yaw_or
time_ros = rospy.get_time()
data_per_iter= {
"time" : time_ros,
"x" : neuronbot2_x,
"y" : neuronbot2_y,
"t" : neuronbot2_t
}
self.gazebostates_tot_data['data'].append(data_per_iter)
def cb_amcl(self, data):
self.amclmsg = data
amclx = data.pose.pose.position.x
amcly = data.pose.pose.position.y
x_or = data.pose.pose.orientation.x
y_or = data.pose.pose.orientation.y
z_or = data.pose.pose.orientation.z
w_or = data.pose.pose.orientation.w
orientation_list = [x_or, y_or, z_or, w_or]
(roll_or, pitch_or, yaw_or) = euler_from_quaternion (orientation_list)
amclt = yaw_or
time_ros = rospy.get_time()
data_per_iter= {
"time" : time_ros,
"x" : amclx,
"y" : amcly,
"t" : amclt
}
self.amcl_tot_data['data'].append(data_per_iter)
def cb_particle(self, data):
self.particlesmsg = data
time_ros = rospy.get_time()
data_per_iter= {
"time" : time_ros,
"particles" : []
}
particle_len = len(self.particlesmsg.poses)
for ptcl_i in range(particle_len):
ptcl_i_x = self.particlesmsg.poses[ptcl_i].position.x
ptcl_i_y = self.particlesmsg.poses[ptcl_i].position.y
x_or = self.particlesmsg.poses[ptcl_i].orientation.x
y_or = self.particlesmsg.poses[ptcl_i].orientation.y
z_or = self.particlesmsg.poses[ptcl_i].orientation.z
w_or = self.particlesmsg.poses[ptcl_i].orientation.w
orientation_list = [x_or, y_or, z_or, w_or]
(roll_or, pitch_or, yaw_or) = euler_from_quaternion (orientation_list)
ptcl_i_t = yaw_or
ptcl_i_data = {
"x": ptcl_i_x,
"y": ptcl_i_y,
"t":ptcl_i_t,
}
data_per_iter["particles"].append(ptcl_i_data)
self.ptcl_tot_data["data"].append(data_per_iter)
if __name__ == "__main__":
print("python is running")
try:
# ROS Init
rospy.init_node('recording_particles', anonymous=True, disable_signals=True)
robot_obj = Robot()
# rospy.spin()
x = raw_input("type anything to stop\n")
print("Shutting down. Do not kill the process\n")
print("unregistering.....")
robot_obj.unregister_all_subscriber()
print("saving amcl poses......\n")
data_dict = robot_obj.amcl_tot_data.copy()
to_json = json.dumps(data_dict)
with open('data_ros_amcl.json', 'w') as outfile:
json.dump(data_dict, outfile, indent=4)
print("saving gazebo states.....\n")
data_dict = robot_obj.gazebostates_tot_data.copy()
to_json = json.dumps(data_dict)
with open('data_ros_gazebo_pos.json', 'w') as outfile:
json.dump(data_dict, outfile, indent=4)
print("saving particles.....\n")
data_dict = robot_obj.ptcl_tot_data.copy()
to_json = json.dumps(data_dict)
with open('data_ros_particles.json', 'w') as outfile:
json.dump(data_dict, outfile, indent=4)
raise KeyboardInterrupt
except KeyboardInterrupt:
print("shutting down recording_particles.py")
sys.exit()
```
# Sample from probability distribution
```c=
clear;
Action_space = [1;2;3;4;5;6;7;8];
prob = [
0.0283;
0.1327;
0.7246;
0.0317;
0.0504;
0.0038;
0.0032;
0.0253];
% s = RandStream('mlfg6331_64');
for i=1:100
seq = datasample(Action_space,1,'Weights',prob);
if abs(seq - 3) > 0
disp("different");
end
end
```
# Initializing weight
```c=
fullyConnectedLayer(criticLayerSizes(2), 'Name', 'CriticStateFC2', ...
'Weights',2/sqrt(criticLayerSizes(1))*(rand(criticLayerSizes(2),criticLayerSizes(1))-0.5), ...
'Bias',2/sqrt(criticLayerSizes(1))*(rand(criticLayerSizes(2),1)-0.5))
```
# Python ROS publish
```python=
#!/home/zee/Manually_Installed/anaconda3/envs/ros1/bin/python
import rospy
import sys
from std_msgs.msg import Float64
class Robot :
def __init__(self):
self.fingerpub = rospy.Publisher('/j2n6s300/finger_tip_1_position_controller/command', Float64, queue_size=10)
self.fingermsg = Float64
def finger_setPoint(self, value):
self.fingermsg = value
self.fingerpub.publish(self.fingermsg)
print('init done')
if __name__ == "__main__":
print("python is running")
try:
# ROS Init
rospy.init_node('recording_particles', anonymous=True, disable_signals=True)
robot_obj = Robot()
# rospy.spin()
print("masukkan setpoint baru")
num1 = float(input())
robot_obj.finger_setPoint(num1);
raise KeyboardInterrupt
except KeyboardInterrupt:
print("shutting down recording_particles.py")
sys.exit()
```
---
###### tags: `notes`
Sign in with Wallet
Connect another wallet