course files uploaded

Author: eetuna

.gitignore

@@ -0,0 +1,28 @@
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app

README.md

@@ -0,0 +1,2 @@
+# Modern_Robotics
+All code/projects created for EECS 397: Modern Robotics

action_server_project/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(action_server_project)
+
+find_package(catkin_simple REQUIRED)
+
+catkin_simple()
+
+# example boost usage
+# find_package(Boost REQUIRED COMPONENTS system thread)
+
+# C++0x support - not quite the same as final C++11!
+# SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+
+# Libraries
+# cs_add_library(my_lib src/my_lib.cpp)   
+
+# Executables
+cs_add_executable(amplitude_frequency_action_client src/amplitude_frequency_action_client.cpp)
+cs_add_executable(velocity_commander_action_server src/velocity_commander_action_server.cpp)
+cs_add_executable(controller src/controller.cpp)
+cs_add_executable(simulator src/simulator.cpp)
+
+cs_install()
+cs_export()
+    

action_server_project/README.md

@@ -0,0 +1,18 @@
+# Action Server Project
+
+This package manipulates the velocity of an object by controlling the force placed on the object. A action client node (amplitude_frequency_action_client.cpp) get user input to determine an amplitude, a frequency, and a number of cycles (periods). The client then requests that the amplitude, frequency, and number of cycles in the action service node (velocity_commander_action_server.cpp) are changed to match. The action server uses the amplitude and frequency to calculate a sine wave and uses the number of cycles to determine how many periods to run for. The sine wave is then calculated with the given amplitude and frequency. The sine wave represents what the velocity should be. This is the velocity command (vel_cmd). This is handed off to the controller which calculates the force needed in order to increase/decrease the current velocity to match the vel_cmd. The simulator then changes the current velocity based on the calculated force.
+
+## Example usage
+In command line:
+roscd
+roscore
+
+In a second command line prompt:
+roslaunch action_server_project action_server_project.launch
+
+In a third command line prompt:
+rqt_plot
+
+In a fourth command line prompt:
+rosrun action_server_project amplitude_frequency_action_client
+    

action_server_project/action/amplitude_frequency_msg.action

@@ -0,0 +1,10 @@
+#goal
+float64 requested_amplitude
+float64 requested_frequency
+int32 requested_num_cycles
+---
+#result
+bool goal_succeeded
+---
+#feedback
+int32 fdbk

action_server_project/action_server_project.launch

@@ -0,0 +1,6 @@
+<launch>
+<!-- <node name= "amplitude_frequency_action_client" pkg= "action_server_project" type= "amplitude_frequency_action_client" /> -->
+<node name= "velocity_commander_action_server" pkg= "action_server_project" type= "velocity_commander_action_server" />
+<node name= "controller" pkg= "action_server_project" type= "controller" />
+<node name= "simulator" pkg= "action_server_project" type= "simulator" />
+</launch>

action_server_project/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package>
+  <name>action_server_project</name>
+  <version>0.0.0</version>
+  <description>The action_server_project package</description>
+  
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">user</maintainer>
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://ros.org/wiki/jacobian_publisher</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+    <build_depend>message_generation</build_depend>
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <run_depend>message_runtime</run_depend>
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <build_depend>actionlib</build_depend>
+  <build_depend>simple_action_client</build_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+<build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+<run_depend>std_msgs</run_depend>
+<run_depend>actionlib</run_depend>
+<run_depend>simple_action_client</run_depend>
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+  </export>
+</package>
+    

action_server_project/src/amplitude_frequency_action_client.cpp

@@ -0,0 +1,68 @@
+#include <ros/ros.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/Int32.h>
+#include <actionlib/client/simple_action_client.h>
+#include <action_server_project/amplitude_frequency_msgAction.h>
+
+// This function will be called once when the goal completes
+// this is optional, but it is a convenient way to get access to the "result" message sent by the server
+void doneCb(const actionlib::SimpleClientGoalState& state, const action_server_project::amplitude_frequency_msgResultConstPtr& result) {
+    if(result->goal_succeeded){
+        ROS_INFO("Goal has been succeeded! Yay!");
+    } else {
+        ROS_INFO("Goal did not succeed... boooo");
+    }
+}
+
+int main(int argc, char** argv) {
+        ros::init(argc, argv, "amplitude_frequency_action_client_node"); //name of this node
+        
+        action_server_project::amplitude_frequency_msgGoal goal;
+        
+        actionlib::SimpleActionClient<action_server_project::amplitude_frequency_msgAction> action_client("amplitude_frequency_action", true);
+        
+        // attempt to connect to the server:
+        ROS_INFO("waiting for server: ");
+        bool server_exists = action_client.waitForServer(ros::Duration(5.0)); // wait for up to 5 seconds
+        //something odd in above: does not seem to wait for 5 seconds, but returns rapidly if server not running
+        //bool server_exists = action_client.waitForServer(); //wait forever
+
+        if (!server_exists) {
+            ROS_WARN("could not connect to server; halting");
+            return 0; // bail out
+        }
+        
+        ROS_INFO("connected to action server");  // if here, then we connected to the server;
+
+        while(true) {
+            std_msgs::Float64 amplitude;
+            std_msgs::Float64 frequency;
+            std_msgs::Int32 numCycles;
+            std::cout << "Please enter in a desired amplitude: ";
+            std::cin >> amplitude.data;
+
+            std::cout << "Please enter in a desired frequency: ";
+            std::cin >> frequency.data;
+
+            std::cout << "Please enter in a desired number of cycles: ";
+            std::cin >> numCycles.data;
+
+            //set the amplitude/frequency/number of cycles to be equal to user input and then send the goal
+            goal.requested_amplitude = amplitude.data;
+            goal.requested_frequency = frequency.data;
+            goal.requested_num_cycles = numCycles.data;
+            action_client.sendGoal(goal,&doneCb);
+            
+            bool finished_before_timeout = action_client.waitForResult(ros::Duration(5.0));
+            //bool finished_before_timeout = action_client.waitForResult(); // wait forever...
+            if (!finished_before_timeout) {
+                ROS_WARN("giving up waiting on result for goal");
+                return 0;
+            } else {
+              //if here, then server returned a result to us
+            }
+        
+        }
+
+    return 0;
+}

action_server_project/src/controller.cpp

@@ -0,0 +1,53 @@
+// minimal_controller node: 
+// wsn example node that both subscribes and publishes--counterpart to minimal_simulator 
+// subscribes to "velocity" and publishes "force_cmd" 
+// subscribes to "vel_cmd" 
+#include <ros/ros.h> 
+#include <std_msgs/Float64.h> 
+//global variables for callback functions to populate for use in main program 
+std_msgs::Float64 g_velocity;
+std_msgs::Float64 g_vel_cmd;
+std_msgs::Float64 g_force; // this one does not need to be global... 
+
+void myCallbackVelocity(const std_msgs::Float64& message_holder) {
+    // check for data on topic "velocity" 
+    ROS_INFO("received velocity value is: %f", message_holder.data);
+    g_velocity.data = message_holder.data; // post the received data in a global var for access by main prog. 
+}
+
+void myCallbackVelCmd(const std_msgs::Float64& message_holder) {
+    // check for data on topic "vel_cmd" 
+    ROS_INFO("received velocity command value is: %f", message_holder.data);
+    g_vel_cmd.data = message_holder.data; // post the received data in a global var for access by main prog. 
+}
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "velocity_controller"); //name this node 
+    // when this compiled code is run, ROS will recognize it as a node called "minimal_controller" 
+    ros::NodeHandle nh; // node handle 
+    //create 2 subscribers: one for state sensing (velocity) and one for velocity commands 
+    ros::Subscriber my_subscriber_object1 = nh.subscribe("velocity", 1, myCallbackVelocity);
+    ros::Subscriber my_subscriber_object2 = nh.subscribe("vel_cmd", 1, myCallbackVelCmd);
+    //publish a force command computed by this controller; 
+    ros::Publisher my_publisher_object = nh.advertise<std_msgs::Float64>("force_cmd", 1);
+    double Kv = 1.0; // velocity feedback gain 
+    double dt_controller = 0.1; //specify 10Hz controller sample rate (pretty slow, but illustrative) 
+    double sample_rate = 1.0 / dt_controller; // compute the corresponding update frequency 
+    ros::Rate naptime(sample_rate); // use to regulate loop rate 
+    g_velocity.data = 0.0; //initialize velocity to zero 
+    g_force.data = 0.0; // initialize force to 0; will get updated by callback 
+    g_vel_cmd.data = 0.0; // init velocity command to zero 
+    double vel_err = 0.0; // velocity error
+    // enter the main loop: get velocity state and velocity commands 
+    // compute command force to get system velocity to match velocity command 
+    // publish this force for use by the complementary simulator 
+    while (ros::ok()) {
+        vel_err = g_vel_cmd.data - g_velocity.data; // compute error btwn desired and actual velocities 
+        g_force.data = Kv*vel_err; //proportional-only velocity-error feedback defines commanded force 
+        my_publisher_object.publish(g_force); // publish the control effort computed by this controller
+        ROS_INFO("force command = %f", g_force.data);
+        ros::spinOnce(); //allow data update from callback; 
+        naptime.sleep(); // wait for remainder of specified period; 
+    }
+    return 0; // should never get here, unless roscore dies 
+} 

action_server_project/src/simulator.cpp

@@ -0,0 +1,40 @@
+// minimal_simulator node: 
+// wsn example node that both subscribes and publishes 
+// does trivial system simulation, F=ma, to update velocity given F specified on topic "force_cmd" 
+// publishes velocity on topic "velocity" 
+#include <ros/ros.h> 
+#include <std_msgs/Float64.h> 
+std_msgs::Float64 g_velocity;
+std_msgs::Float64 g_force;
+
+void myCallback(const std_msgs::Float64& message_holder) {
+    // checks for messages on topic "force_cmd" 
+    ROS_INFO("received force value is: %f", message_holder.data);
+    g_force.data = message_holder.data; // post the received data in a global var for access by main prog. 
+}
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "velocity_simulator"); //name this node 
+    // when this compiled code is run, ROS will recognize it as a node called "minimal_simulator" 
+    ros::NodeHandle nh; // node handle 
+    //create a Subscriber object and have it subscribe to the topic "force_cmd" 
+    ros::Subscriber my_subscriber_object = nh.subscribe("force_cmd", 1, myCallback);
+    //simulate accelerations and publish the resulting velocity; 
+    ros::Publisher my_publisher_object = nh.advertise<std_msgs::Float64>("velocity", 1);
+    double mass = 1.0;
+    double dt = 0.01; //10ms integration time step 
+    double sample_rate = 1.0 / dt; // compute the corresponding update frequency 
+    ros::Rate naptime(sample_rate);
+    g_velocity.data = 0.0; //initialize velocity to zero 
+    g_force.data = 0.0; // initialize force to 0; will get updated by callback 
+    while (ros::ok()) {
+        g_velocity.data = g_velocity.data + (g_force.data / mass) * dt; // Euler integration of acceleration 
+        my_publisher_object.publish(g_velocity); // publish the system state (trivial--1-D) 
+        ROS_INFO("velocity = %f", g_velocity.data);
+        ros::spinOnce(); //allow data update from callback 
+        naptime.sleep(); // wait for remainder of specified period; this loop rate is faster than 
+        // the update rate of the 10Hz controller that specifies force_cmd 
+        // however, simulator must advance each 10ms regardless 
+    }
+    return 0; // should never get here, unless roscore dies
+}