diff --git a/OOPS/bank_accound.cpp b/OOPS/bank_accound.cpp new file mode 100644 index 0000000..9095150 --- /dev/null +++ b/OOPS/bank_accound.cpp @@ -0,0 +1,290 @@ +#include +#include +#include +#include // For rand() function +using namespace std; + +class Statement { +private: + double amount; + string transfer_type; + double Amount_flow; + +public: + Statement(string desc, double amt) { + transfer_type = desc; + amount = amt; + } + void getDetails() { + cout << "Description: " << transfer_type << endl; + cout << "Amount: $" << amount << endl; + } +}; + +class Bank_Account { +protected: + string Account_type; + double Balance; + Statement** statements; + int numStatements; + +public: + long Account_No; + + Bank_Account(int id, string type, double openingBalance) { + Account_No = id; // Account number is provided during account creation + Account_type = type; + Balance = openingBalance; + numStatements = 0; + statements = new Statement*[100]; + } + + virtual ~Bank_Account() { + for (int i = 0; i < numStatements; ++i) { + delete statements[i]; + } + delete[] statements; + } + + string getAccountType() const { + return Account_type; + } + + void addStatement(const Statement& statement) { + statements[numStatements++] = new Statement(statement); + } + + void viewStatements() const { + cout << "----- Statements for Account ID: " << Account_No << " -----" << endl; + for (int i = 0; i < numStatements; ++i) { + statements[i]->getDetails(); + cout << endl; + } + } + + void deposit(double amount); + bool withdraw(double amount); +}; + +bool Bank_Account::withdraw(double amount) { + if (Account_type == "Savings" && amount > 200000) { + cout << "Savings account cannot withdraw more than 200000 at a time." << endl; + return false; + } + + if (Balance >= amount) { + Balance -= amount; + addStatement(Statement("Withdrawal", -amount)); + cout << "Withdrawal of " << amount << " successful. New balance: " << Balance << endl; + return true; + } else { + cout << "Insufficient funds." << endl; + return false; + } +} + +class Savings_Account : public Bank_Account { +private: + float Interest_Rate; + const int Upper_Transcation_Limit = 2000000; + +public: + Savings_Account(long accountNo) : Bank_Account(accountNo, "Savings", 0) {} + + float getInterestRate() { + return Interest_Rate; + } + + float setInterestRate(float interestrate) { + if (interestrate > 6) { + cout << "Max interest rate is 6"; + } else { + Interest_Rate = interestrate; + } + return Interest_Rate; + } + + ~Savings_Account() {} +}; + +class Current_Account : public Bank_Account { +private: + const float Interest_Rate = 0; + +public: + Current_Account(long accountNo) : Bank_Account(accountNo, "Current", 0) {} + + float getInterestRate() { + throw ("Interest Rate is 0"); + } + + ~Current_Account() {} +}; + +class Bank_Account_Holder { +private: + string username; + string password; + Bank_Account** list; + int capacity; + int numofAccounts; + +public: + Bank_Account_Holder(string name, string passwd) { + username = name; + password = passwd; + capacity = 100; + numofAccounts = 0; + list = new Bank_Account*[capacity]; + }; + + void changepasswd(string passwd); + void createAccount(string accounttype); + + Savings_Account* get_savingsAccount(long accountId); + Current_Account* get_currentAccount(long accountId); +}; + +void Bank_Account_Holder::changepasswd(string passwd) { + password = passwd; +} + +void Bank_Account_Holder::createAccount(string accounttype) { + if (numofAccounts < capacity) { + long accountNo = rand() % 900000000 + 100000000; // Generate a random account number + if (accounttype == "Savings") { + list[numofAccounts++] = new Savings_Account(accountNo); + cout << "Savings account created with account number: " << accountNo << endl; + } else if (accounttype == "Current") { + list[numofAccounts++] = new Current_Account(accountNo); + cout << "Current account created with account number: " << accountNo << endl; + } + } else { + cout << "Maximum account limit reached." << endl; + } +} + +Savings_Account* Bank_Account_Holder::get_savingsAccount(long accountId) { + for (int i = 0; i < numofAccounts; ++i) { + if (list[i]->Account_No == accountId && list[i]->getAccountType() == "Savings") { + return dynamic_cast(list[i]); + } + } + return nullptr; +} + +Current_Account* Bank_Account_Holder::get_currentAccount(long accountId) { + for (int i = 0; i < numofAccounts; ++i) { + if (list[i]->Account_No == accountId && list[i]->getAccountType() == "Current") { + return dynamic_cast(list[i]); + } + } + return nullptr; +} + +void Bank_Account::deposit(double money) { + Balance += money; + addStatement(Statement("Deposit", money)); +} + +int main() { + Bank_Account_Holder accountHolder("Ayush", "password123"); + + // Main menu + int choice; + do { + cout << "\nBanking System Menu\n"; + cout << "1. Create Savings Account\n"; + cout << "2. Create Current Account\n"; + cout << "3. Deposit\n"; + cout << "4. Withdraw\n"; + cout << "5. View Statements\n"; + cout << "6. Exit\n"; + cout << "Enter your choice: "; + cin >> choice; + + if (choice == 1) { + accountHolder.createAccount("Savings"); + } else if (choice == 2) { + accountHolder.createAccount("Current"); + } else if (choice == 3) { + long accountNumber; + double amount; + string Accounttype; + cout << "Enter account type Saving/Current: "; + cin >> Accounttype; + cout << "Enter account number: "; + cin >> accountNumber; + cout << "Enter amount to deposit: "; + cin >> amount; + if (Accounttype == "Savings") { + Savings_Account* account = accountHolder.get_savingsAccount(accountNumber); + if (account != nullptr) { + account->deposit(amount); + } else { + cout << "Savings account with given account number not found." << endl; + } + } else if (Accounttype == "Current") { + Current_Account* account = accountHolder.get_currentAccount(accountNumber); + if (account != nullptr) { + account->deposit(amount); + } else { + cout << "Current account with given account number not found." << endl; + } + } + } else if (choice == 4) { + long accountNumber; + double amount; + string Accounttype; + cout << "Enter account type Saving/Current: "; + cin >> Accounttype; + cout << "Enter account number: "; + cin >> accountNumber; + cout << "Enter amount to withdraw: "; + cin >> amount; + if (Accounttype == "Savings") { + Savings_Account* account = accountHolder.get_savingsAccount(accountNumber); + if (account != nullptr) { + account->withdraw(amount); + } else { + cout << "Savings account with given account number not found." << endl; + } + } else if (Accounttype == "Current") { + Current_Account* account = accountHolder.get_currentAccount(accountNumber); + if (account != nullptr) { + account->withdraw(amount); + } else { + cout << "Current account with given account number not found." << endl; + } + } + } else if (choice == 5) { + long accountNumber; + string Accounttype; + cout << "Enter account type Saving/Current: "; + cin >> Accounttype; + cout << "Enter account number: "; + cin >> accountNumber; + if (Accounttype == "Savings") { + Savings_Account* account = accountHolder.get_savingsAccount(accountNumber); + if (account != nullptr) { + account->viewStatements(); + } else { + cout << "Savings account with given account number not found." << endl; + } + } else if (Accounttype == "Current") { + Current_Account* account = accountHolder.get_currentAccount(accountNumber); + if (account != nullptr) { + account->viewStatements(); + } else { + cout << "Current account with given account number not found." << endl; + } + } + } else if (choice == 6) { + cout << "Have a good Day" << endl << "Exiting..."; + } else { + cout << "Invalid choice. Please enter a number from 1 to 6." << endl; + } + } while (choice != 6); + + return 0; +} diff --git a/README.md b/README.md index a381c03..608322e 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,19 @@ -# Induction_Y23 -Induction Assignments and resources for Y23 +I have created two files name ftpserver and ftp client + +From client we can perform tasks like storing the file on server, retriving the file from server, listing all the files already present in server + +Storing the file to server, retriving it from server along with list function work completely fine + +Whenever a client connects to the server it first ask username. If username is already present in the list that it asks for password and if it matches the useranme password key present in the fserver it lets the userin + +Also if username is not already listed, he can create a new user. + +Problem with this code is that it can not handle exceptions for ex. file that you want to transfer to server , if its not present in your working directory it will give error andget stuck midway. + +It will work if user give the right set of command. like to store a file in server the file, the client has to write the right filename or my code won't work + +I am unable to do this exception handling + +I have also tried making admin. For that i created a new python file named admin. But integrating it with server and client was problem as the code got stuck midway due to some improper aapplication of threading(i guess) + +You can check the integrated code that does not work its named ftpserver copy and ftp client copy.py diff --git a/admin.py b/admin.py new file mode 100644 index 0000000..aed7844 --- /dev/null +++ b/admin.py @@ -0,0 +1,47 @@ +import socket +import threading +admin = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + +IP_ADDR = socket.gethostbyname(socket.gethostname()) +port = 1234 + +admin.connect((IP_ADDR,port)) + +print(f"Server is listening on {IP_ADDR}:{port}") + +admin.send("ADMIN".encode()) + +if __name__ == "__main__": + print("Welcome to the Server. Please follow below commands:") + print("To ADD a user: ADD") + print("To DELETE a user : DEL") + print("To BAN a user: BAN") + print("To UNBAN a user: UNBAN") + while True: + command = input("Enter the command: ") + if command == "ADD": + admin.send("ADD".encode()) + username= input("Enter username you want to add: ") + admin.send(username.encode()) + passwd=input("Enter the passwd for the username: ") + admin.send(passwd.encode()) + + elif command == "DEL": + admin.send("DEL".encode()) + username= input("Enter username you want to add: ") + admin.send(username.encode()) + + elif command=="BAN": + admin.send("BAN".encode()) + username= input("Enter username you want to add: ") + admin.send(username.encode()) + + elif command == "UNBAN": + admin.send("UNBAN".encode()) + username= input("Enter username you want to add: ") + admin.send(username.encode()) + + else: + print("The given command is wrong") + break + diff --git a/different_world_simulation (2).launch b/different_world_simulation (2).launch new file mode 100644 index 0000000..5b6b524 --- /dev/null +++ b/different_world_simulation (2).launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/different_world_simulation.launch b/different_world_simulation.launch new file mode 100644 index 0000000..d9ee886 --- /dev/null +++ b/different_world_simulation.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/first.txt b/first.txt new file mode 100644 index 0000000..2ef2a81 --- /dev/null +++ b/first.txt @@ -0,0 +1 @@ +THIS IS A TEST FILE TO CHECK THE TRANSFERING OF FILE FROM CLIENT TO SERVER \ No newline at end of file diff --git a/ftpclient copy.py b/ftpclient copy.py new file mode 100644 index 0000000..c3c79f9 --- /dev/null +++ b/ftpclient copy.py @@ -0,0 +1,97 @@ +import socket +import threading + +username=input("Enter a username: ") + + +client = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +IP_ADDR = socket.gethostbyname(socket.gethostname()) +port = 1234 +client.connect((IP_ADDR,port)) +print("yes") +client.send("USER".encode()) +print("yes") + +client.send(username.encode()) +print("yes") + +connecting_message= client.recv(1024).decode() +print("yes") + + +if connecting_message =="Password?": + passwd=input("Enter the password: ") + client.send(passwd.encode()) +elif connecting_message=="New Account": + print(f"New account is created with username {username}") + passwd=input("Enter the password: ") + client.send(passwd.encode()) + passwd1 =input("Enter above passwd again: ") + client.send(passwd1.encode()) + +def file_transfer(): + message="STORE FILE" + client.send(message.encode()) + filename = input("Please Enter the file name you want to add :") + filename1= input("please enter the new filename with which you want to store above file in server: ") + client.send(filename1.encode()) + try: + file= open(filename, 'r') + data =file.read() + # while data: + client.send(data.encode()) + # data = file.read() + file.close() + except: print("You have entered a wrong file address") + +def file_retriving(): + try: + message="Retriving FILE" + client.send(message.encode()) + filename =input("Please input the file name you want to retrieve") + client.send(filename.encode()) + data=client.recv(1024).decode() + print(data) + filename = input("Please input the name with which you want to store it in your device: ") + file = open(filename, 'w') + # while(data): + # file.write(str(data)) + except: + print("Given file is not present on server") +def list1(): + message= "list" + client.send(message.encode()) + list23=[] + data=client.recv(1024).decode() + + while(data!="done"): + list23.append(data) + data=client.recv(1024).decode() + print(list23) + +if __name__ == "__main__": + print("Welcome to the Server. Please follow below commands:") + print("To store the file on server give command : STOR") + print("To retrieve a file from the server give command : RETR") + print("To list the file present on server: LIST") + print("To quit the server: QUIT") + + while True: + command = input("Enter the command: ") + if command == "STOR": + file_transfer() + elif command == "RETR": + file_retriving() + elif command=="LIST": + list1() + elif command == "QUIT": + print("Adios amigo!!!!!") + break + else: print("The given command is wrong") + + + + + + + diff --git a/ftpclient.py b/ftpclient.py new file mode 100644 index 0000000..142f80b --- /dev/null +++ b/ftpclient.py @@ -0,0 +1,89 @@ +import socket +import threading + +username=input("Enter a username: ") + + +client = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +IP_ADDR = socket.gethostbyname(socket.gethostname()) +port = 1234 +client.connect((IP_ADDR,port)) +client.send("USER".encode()) +client.send(username.encode()) +connecting_message= client.recv(1024).decode() + +if connecting_message =="Password?": + passwd=input("Enter the password: ") + client.send(passwd.encode()) +elif connecting_message=="New Account": + print(f"New account is created with username {username}") + passwd=input("Enter the password: ") + client.send(passwd.encode()) + passwd1 =input("Enter above passwd again: ") + client.send(passwd1.encode()) + +def file_transfer(): + message="STORE FILE" + client.send(message.encode()) + filename = input("Please Enter the file name you want to add :") + filename1= input("please enter the new filename for the server: ") + client.send(filename1.encode()) + try: + file= open(filename, 'r') + data =file.read() + # while data: + client.send(data.encode()) + # data = file.read() + file.close() + except: print("You have entered a wrong file address") + +def file_retriving(): + try: + message="Retriving FILE" + client.send(message.encode()) + filename =input("Please input the file name you want to retrieve") + client.send(filename.encode()) + data=client.recv(1024).decode() + filename = input("Please input the name with which you want to store it in your device: ") + file = open(filename, 'w') + # while(data): + file.write(str(data)) + except: + print("Given file is not present on server") +def list1(): + message= "list" + client.send(message.encode()) + list23=[] + data=client.recv(1024).decode() + + while(data!="done"): + list23.append(data) + data=client.recv(1024).decode() + print(list23) + +if __name__ == "__main__": + print("Welcome to the Server. Please follow below commands:") + print("To store the file on server give command : STOR") + print("To retrieve a file from the server give command : RETR") + print("To list the file present on server: LIST") + print("To quit the server: QUIT") + + while True: + command = input("Enter the command: ") + if command == "STOR": + file_transfer() + elif command == "RETR": + file_retriving() + elif command=="LIST": + list1() + elif command == "QUIT": + print("Adios amigo!!!!!") + break + else: print("The given command is wrong") + + + + + + + diff --git a/ftpserver copy.py b/ftpserver copy.py new file mode 100644 index 0000000..3357ab7 --- /dev/null +++ b/ftpserver copy.py @@ -0,0 +1,157 @@ +import socket +import threading +server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + +IP_ADDR = socket.gethostbyname(socket.gethostname()) +port = 1234 + +server.bind((IP_ADDR,port)) + +server.listen() +print(f"Server is listening on {IP_ADDR}:{port}") + +users={} +clients=[] +file_names=[] +ban=[] + +def new_client(): + while True: + client, address = server.accept() + print("yes") + role=client.recv(1024).decode() + print("yes") + + if(role=="ADMIN"): + print(f"\nConnection received from {address}") + print(f"ADMIN is connected to the server") + print(f"Active Connection : {threading.active_count()}") + t = threading.Thread(target=admin_handle, args=(client,)) + t.start() + + + else: + print("yes") + print(f"\nConnection received from {address}") + username=client.recv(1024).decode() + clients.append(client) + print("yes") + + if (username in users.keys() and username not in ban): + passwd="Password?" + client.send(passwd.encode()) + passwd = client.recv(1024).decode() + + if users[username]==passwd: + print(f"Welcome aboard {username}") + else: + print(f"Incorrect Password") + client.close() + break + elif username in ban: + print(f"{username} is banned from the server") + else: + message="New Account" + client.send(message.encode()) + passwd = client.recv(1024).decode() + passwd1= client.recv(1024).decode() + if(passwd==passwd1) : + users[username]=passwd + print(f"New account created successfully with username : {username}") + else: + client.close() + print(f"Password did not match") + + print(f"Active Connection : {threading.active_count()}") + t = threading.Thread(target=handle_client, args=(client,)) + t.start() + +def handle_client(client): + while True: + try: + message = client.recv(1024).decode() + if message== "STORE FILE": + try: + filename=client.recv(1024).decode() + # if filename in file_names: + # print("File already present on server") + # else: + file_names.append(filename) + file= open(filename ,'w') + + data=client.recv(1024).decode() + print(f"{filename} added") + # while(data): + + # if not data: + # break + # else: + file.write(data) + # data=client.recv(1024).decode() + file.close() + except: + print(" Fileis already present on server") + + + elif message== "Retriving FILE": + filename=client.recv(1024).decode() + if filename in file_names: + file = open(filename , 'r') + data=file.read() + print(data) + # while(data): + client.send(data.encode()) + # data=file.read() + file.close() + else: + print("No Such file present") + + + elif message=="list": + length=len(file_names) + while(length): + client.send(file_names[length-1].encode()) + length-=1 + client.send("done".encode()) + except: + client.close() + break +def admin_handle(admin): + while True: + message = admin.recv(1024).decode() + if message == "DEL": + username=admin.recv(1024).decode() + del users[username] + print(f"{username} has been deleted from the server by admin") + + elif message == "ADD": + username=admin.recv(1024).decode() + passwd=admin.recv(1024).decode() + if username not in ban: + users[username]=passwd + print(f"{username} has been added to the server") + + + elif message == "BAN": + username=admin.recv(1024).decode() + ban.append(username) + print(f"{username} has been banned from the server") + + + elif message =="UNBAN": + username=admin.recv(1024).decode() + ban.remove(username) + print(f"{username} has been unbanned from the server") + + + + + + + + + + + +if __name__ == "__main__": + new_client() \ No newline at end of file diff --git a/ftpserver.py b/ftpserver.py new file mode 100644 index 0000000..cd0118a --- /dev/null +++ b/ftpserver.py @@ -0,0 +1,110 @@ +import socket +import threading +server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + +IP_ADDR = socket.gethostbyname(socket.gethostname()) +port = 1234 + +server.bind((IP_ADDR,port)) + +server.listen() +print(f"Server is listening on {IP_ADDR}:{port}") + +users={} +clients=[] +file_names=[] + +def new_client(): + while True: + client, address = server.accept() + # role=client.recv(1024).decode() + # if(role=="ADMIN"): + # print(f"ADMIN is connected to the server") + + + print(f"\nConnection received from {address}") + username=client.recv(1024).decode() + clients.append(client) + if username in users.keys(): + passwd="Password?" + client.send(passwd.encode()) + passwd = client.recv(1024).decode() + + if users[username]==passwd: + print(f"Welcome aboard {username}") + else: + print(f"Incorrect Password") + client.close() + break + else: + message="New Account" + client.send(message.encode()) + passwd = client.recv(1024).decode() + passwd1= client.recv(1024).decode() + if(passwd==passwd1) : + users[username]=passwd + print(f"New account created successfully with username : {username}") + else: + client.close() + print(f"Password did not match") + + print(f"Active Connection : {threading.active_count()}") + t = threading.Thread(target=handle_client, args=(client,)) + t.start() + +def handle_client(client): + while True: + try: + message = client.recv(1024).decode() + if message== "STORE FILE": + try: + filename=client.recv(1024).decode() + # if filename in file_names: + # print("File already present on server") + # else: + file_names.append(filename) + file= open(filename ,'w') + + data=client.recv(1024).decode() + print(f"{filename} added") + # while(data): + + # if not data: + # break + # else: + file.write(data) + # data=client.recv(1024).decode() + file.close() + except: + print(" Fileis already present on server") + + + elif message== "Retriving FILE": + filename=client.recv(1024).decode() + if filename in file_names: + file = open(filename , 'r') + data=file.read() + print(data) + # while(data): + client.send(data.encode()) + # data=file.read() + file.close() + else: + print("No Such file present") + + + elif message=="list": + length=len(file_names) + while(length): + client.send(file_names[length-1].encode()) + length-=1 + client.send("done".encode()) + except: + client.close() + break + + + + +if __name__ == "__main__": + new_client() \ No newline at end of file diff --git a/param.yaml b/param.yaml new file mode 100644 index 0000000..ceb0e42 --- /dev/null +++ b/param.yaml @@ -0,0 +1,2 @@ +topic_name: \scan +queue_size: 100 diff --git a/readme.md b/readme.md new file mode 100644 index 0000000..b4e63f0 --- /dev/null +++ b/readme.md @@ -0,0 +1,13 @@ +## ROS ASSIGNMENT 2 +Used catkin_create_pkg smb_highlevel_controller roscpp rospy to create a nw package in the workspace used in first assignmnet + +Created a node inside src folder of smb_highlevel_controller pkg that subscribed to the /scan topic. It Subscribe to /scan and then call function scannerCallBack which take the message from topic and find the minimun distance of obstacle from the bot and use ROS_INFO to display that distance on terminal + +Created a parameter file for the node + +For the launch file, I have copied the launch file that was used in first assignment into the smb_highlevel_controllerpkg with some changes. + +In the launch file I have added node that was created earlier than passed laser_enabled argument. + +Further for the last part I have added rviz to the launch file and add an argument to find smb_highlevel_controller so that smb_highlevel_controller can be stimulated on rviz simulator. + diff --git a/ros_assignmnet1.md b/ros_assignmnet1.md new file mode 100644 index 0000000..7849877 --- /dev/null +++ b/ros_assignmnet1.md @@ -0,0 +1,28 @@ +For 1st ques:- +First created a gir folder and extracted smb_commmon zipped folder in it. Then created the workspace folder and inside it a smb_ws named folder. Inside smb_ws folder created a src folder and then inside src folder symlinked the smb_common package in it. Then changed directory to smb_ws and run the command "catkin_make" which created the build and devel folder.. +For 2nd ques:- +Launch the smb_gazebo package using commabd "roslaunch smb_gazebo smb_gazebo.launch". It launched several nodes and a gazebo simulator. +After the package is launched used rosnode list and rostoic list to find the running node and list +Then used rostopic echo /cmd_vel to check what is being published to /cmd_vel topic +Then used rqt_graph to check all nodes and topics +For 3rd ques:- +Used rostopic pub /cmd_vel geometry_msgs/Twist "Linear: X:0.0 + y:1.0 + z:0.0 + angular: + x:0.0 + y:0.0 + z:1.0" +Using this command the bot started moving circle on the gazebo +For 4th ques:- +Have cloned teleop_twist_keyboard git repo through git clone. +Then launches teleop_twist_keyboard node using "rosrun teleop_twist_keyboard teleop_twist_keyboard .py" +Through this command we can move bot through the keys on our keyboard. We can visualize the motion on gazebo +For 5th ques:- +Created a launch file which will launch gazebo launch but with change in world argument to the environmnet given in ques. + + + + + + diff --git a/smb_highlevel_controller_node.cpp b/smb_highlevel_controller_node.cpp new file mode 100644 index 0000000..ad60a77 --- /dev/null +++ b/smb_highlevel_controller_node.cpp @@ -0,0 +1,93 @@ +#include +#include +#include +#include +#include +#include "smb_highlevel_controller/SmbHighlevelController.hpp" + +class Handler { +public: + Handler() : nodeHandle("~"), publisher(), vis_pub(),subscriber(), smbHighlevelController(nodeHandle) { + nodeHandle.param("scan_topic", scan_topic, std::string("/scan")); + nodeHandle.param("queue_size", queue_size, 100); + subscriber = nodeHandle.subscribe(scan_topic, queue_size, &Handler::scanCallback, this); + publisher = nodeHandle.advertise("/cmd_vel", queue_size); + vis_pub = nodeHandle.advertise( "/visualization_marker",100 ); + + nodeHandle.getParam("/Kp_angular", Kp_angular); + nodeHandle.getParam("/Kp_linear", Kp_linear); + } +void Marker1(double x, double y){ + visualization_msgs::Marker marker; + marker.header.frame_id = "base_link"; + marker.header.stamp = ros::Time(); + marker.ns = "my_namespace"; + marker.id = 0; + marker.type = visualization_msgs::Marker::SPHERE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position.x = x; + marker.pose.position.y = y; + marker.pose.position.z = 0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.5; + marker.scale.y = 0.5; + marker.scale.z = 0.5; + marker.color.a = 1.0; // Don't forget to set the alpha! + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + vis_pub.publish( marker ); +} + void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg) { + std::vector ranges = msg->ranges; + float angle_increment = msg->angle_increment; + float angle_min = msg->angle_min; + float min_distance = ranges[0]; + float ray_angle = angle_min; + + for (size_t i = 1; i < ranges.size(); ++i) { + if (ranges[i] < min_distance) { + min_distance = ranges[i]; + ray_angle = angle_min + (i * angle_increment); + } + } + + double linear_velocity = Kp_linear * min_distance; + double angular_velocity = Kp_angular * ray_angle*10; + double angle_rad=ray_angle; + double x=min_distance*cos(angle_rad); + double y=min_distance*sin(angle_rad); + + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = linear_velocity; + cmd_vel.angular.z = angular_velocity; + publisher.publish(cmd_vel); + + ROS_INFO("Smallest distance: %f", min_distance); + ROS_INFO("angle: %f", ray_angle); + Marker1(x,y); + + } + +private: + ros::NodeHandle nodeHandle; + ros::Publisher publisher; + ros::Publisher vis_pub; + ros::Subscriber subscriber; + std::string scan_topic; + int queue_size; + double Kp_linear; + double Kp_angular; + smb_highlevel_controller::SmbHighlevelController smbHighlevelController; +}; + +int main(int argc, char** argv) { + ros::init(argc, argv, "smb_highlevel_controller"); + Handler hd; + ros::spin(); + return 0; +} +