Skip to content

Instantly share code, notes, and snippets.

@icyflame

icyflame/repoCodeAndVehicleCode.diff

Created August 27, 2015 07:38
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save icyflame/62810d8e9a67aa21a272 to your computer and use it in GitHub Desktop.
Save icyflame/62810d8e9a67aa21a272 to your computer and use it in GitHub Desktop.
Download ZIP
Diff generated using `git diff --ignore-space-at-eol` between vehicle code and the code on AUV IIT KGP's repo, as on 26/8/2015
Raw
repoCodeAndVehicleCode.diff
diff --git a/Scripts/launch/goKraken.launch b/Scripts/launch/goKraken.launch
index fea893a..d657ab2 100644
--- a/Scripts/launch/goKraken.launch
+++ b/Scripts/launch/goKraken.launch
@@ -1,16 +1,16 @@
<launch>
<node name="imudata" pkg="sparton" type="sparton.py" />
- <!-- <node name="dvldata" pkg="nqDVL" type="nqDVL.py" /> -->
+ <node name="dvldata" pkg="nqDVL" type="nqDVL,py" />
- <!-- <node name="seaboticConverter" pkg="thruster_converter" type="thrust_convert" /> -->
+ <node name="seaboticConverter" pkg="thruster_converter" type="thruster" />
- <!-- <node name="control_node" pkg="control_server" type="controlServer" args="10 $(find control_server)/parameters/MoveBack.cp
+ <node name="control_node" pkg="control_server" type="controlServer" args="10 $(find control_server)/parameters/MoveBack.cp
$(find control_server)/parameters/MoveForward.cp $(find control_server)/parameters/Pause.cp"/>
- -->
+
<node name="Thruster" pkg="seabotix" type="seabotix.py" />
- <!-- <node name="Thruster" pkg="seabotix" type="seabotix.py" /> -->
+ <node name="Thruster" pkg="seabotix" type="seabotix.py" />
</launch>
\ No newline at end of file
diff --git a/Scripts/testThrusters.sh b/Scripts/testThrusters.sh
deleted file mode 100755
index fb003ab..0000000
--- a/Scripts/testThrusters.sh
+++ /dev/null
@@ -1,3 +0,0 @@
-roscd
-roscd seabotix
-python src/testThrusters.py
diff --git a/contributing.md b/contributing.md
new file mode 100644
index 0000000..1b0df53
--- /dev/null
+++ b/contributing.md
@@ -0,0 +1,62 @@
+## Contributing Guidelines
+
+- Code must be formatted with the `hooks/formatAll.sh` script, before committing to the repository.
+- We use [ArtisticStyle](http://astyle.sourceforge.net/) for code formatting.
+ - Download the package from [SourceForge](http://sourceforge.net/projects/astyle)
+ - Installation guideliens are provided [here](http://astyle.sourceforge.net/install.html)
+- Code guide at a glance:
+
+```cpp
+#include <ros/ros.h>
+
+float someFunction (int firstArgument, float secondArgument)
+{
+ return firstArgument + secondArgument;
+}
+
+int main ()
+{
+ int i;
+ int count = 10;
+ bool condition;
+
+ if (condition)
+ {
+ std::cout << "Print a statement." << std::endl;
+ }
+ else
+ {
+ std::cout << "Print other statement." << std::endl;
+ }
+
+ for (i = 0; i < count; i++)
+ {
+ std::cout << "Index: " << i << endl;
+ }
+
+ switch (i)
+ {
+ case 1:
+ {
+ std::cout << "This was the first case." << std::endl;
+ break;
+ }
+
+ case 2:
+ {
+ std::cout << "This was the second case." << std::endl;
+ break;
+ }
+
+ default:
+ {
+ std::cout << "This was the default case." << std::endl;
+ break;
+ }
+ }
+
+ return 0;
+}
+```
+
+- Check [this file](./vision_stack/task_buoy/src/buoy_server.cpp) to see more example code.
diff --git a/control_system_stack/absolute_rpy_publisher/src/main.py b/control_system_stack/absolute_rpy_publisher/src/main.py
old mode 100755
new mode 100644
index 80a711a..d73be66
--- a/control_system_stack/absolute_rpy_publisher/src/main.py
+++ b/control_system_stack/absolute_rpy_publisher/src/main.py
@@ -1,5 +1,3 @@
-#!/usr/bin/env python
-
'''
Publishing the absolute Roll, Pitch and Yaw
@@ -20,7 +18,8 @@ Publishing the absolute Roll, Pitch and Yaw
- Reference: http://en.wikipedia.org/wiki/File:Yaw_Axis_Corrected.svg
'''
-import roslib;roslib.load_manifest('mission_planner')
+import roslib;roslib.load_manifest('absolute_rpy_publisher')
+import rospy
import rospy
import time
@@ -42,23 +41,16 @@ def imuCallback(imu):
pitch = imu.data[1]
yaw = imu.data[2]
- # print roll, pitch, yaw
# Fix the roll, pitch by subtracting it from 360
roll = 360 - roll
pitch = 360 - pitch
- roll = roll % 360
- pitch = pitch % 360
- yaw = yaw % 360
-
abrpy = absoluteRPY()
abrpy.roll = roll
abrpy.pitch = pitch
abrpy.yaw = yaw
- print roll, pitch, yaw
-
absolute_rpy_publisher.publish(abrpy)
# Store this in a message and publish it
diff --git a/control_system_stack/controller_basic/cfg/cpp/controller_basic/controllerConfig.h b/control_system_stack/controller_basic/cfg/cpp/controller_basic/controllerConfig.h
index 7770729..d8c27f5 100644
--- a/control_system_stack/controller_basic/cfg/cpp/controller_basic/controllerConfig.h
+++ b/control_system_stack/controller_basic/cfg/cpp/controller_basic/controllerConfig.h
@@ -7,6 +7,43 @@
//
// ********************************************************/
+/***********************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ ***********************************************************/
+
+// Author: Blaise Gassend
+
+
#ifndef __controller_basic__CONTROLLERCONFIG_H__
#define __controller_basic__CONTROLLERCONFIG_H__
@@ -273,7 +310,7 @@ public:
double Kd;
//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
double Ki;
-//#line 218 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template"
+//#line 255 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template"
bool __fromMessage__(dynamic_reconfigure::Config &msg)
{
@@ -481,7 +518,7 @@ class controllerConfigStatics
Default.convertParams();
//#line 233 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
__group_descriptions__.push_back(controllerConfig::AbstractGroupDescriptionConstPtr(new controllerConfig::GroupDescription<controllerConfig::DEFAULT, controllerConfig>(Default)));
-//#line 353 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template"
+//#line 390 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template"
for (std::vector<controllerConfig::AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); ++i)
{
diff --git a/control_system_stack/controller_basic/src/controller_basic/cfg/controllerConfig.py b/control_system_stack/controller_basic/src/controller_basic/cfg/controllerConfig.py
index bdcd782..9224445 100644
--- a/control_system_stack/controller_basic/src/controller_basic/cfg/controllerConfig.py
+++ b/control_system_stack/controller_basic/src/controller_basic/cfg/controllerConfig.py
@@ -6,6 +6,40 @@
##
## ********************************************************/
+##**********************************************************
+## Software License Agreement (BSD License)
+##
+## Copyright (c) 2008, Willow Garage, Inc.
+## All rights reserved.
+##
+## Redistribution and use in source and binary forms, with or without
+## modification, are permitted provided that the following conditions
+## are met:
+##
+## * Redistributions of source code must retain the above copyright
+## notice, this list of conditions and the following disclaimer.
+## * Redistributions in binary form must reproduce the above
+## copyright notice, this list of conditions and the following
+## disclaimer in the documentation and/or other materials provided
+## with the distribution.
+## * Neither the name of the Willow Garage nor the names of its
+## contributors may be used to endorse or promote products derived
+## from this software without specific prior written permission.
+##
+## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+## "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+## LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+## FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+## COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+## INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+## BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+## LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+## CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+## LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+## ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+## POSSIBILITY OF SUCH DAMAGE.
+##**********************************************************/
+
from dynamic_reconfigure.encoding import extract_params
inf = float('inf')
diff --git a/control_system_stack/keyboard_control/src/temp.txt b/control_system_stack/keyboard_control/src/temp.txt
deleted file mode 100644
index 29c3db7..0000000
--- a/control_system_stack/keyboard_control/src/temp.txt
+++ /dev/null
@@ -1,207 +0,0 @@
-import roslib;roslib.load_manifest('keyboard_control')
-
-from Tkinter import *
-import rospy
-from std_msgs.msg import String
-from kraken_msgs.msg import thrusterData6Thruster
-from resources import topicHeader
-
-rospy.init_node('keyboard_control_vehicle_node', anonymous=True)
-
-import replicateData
-
-root = Tk()
-
-frame = Frame(root, width=100, height=100)
-
-label_vars = range(6)
-
-for i in range(len(label_vars)):
- label_vars[i] = StringVar()
- label_vars[i].set('empty')
-
-def editGui(data):
-
- global FIRST_ITERATION
- global previous6
- global present6
-
- for i in range(len(data.data)):
- label_vars[i].set(data.data[i])
-
- present6.data[0] = previous6.data[0] = data.data[0]
- present6.data[1] = previous6.data[1] = data.data[1]
- present6.data[2] = previous6.data[2] = data.data[2]
- present6.data[3] = previous6.data[3] = data.data[3]
- present6.data[4] = previous6.data[4] = data.data[4]
- present6.data[5] = previous6.data[5] = data.data[5]
-
- # if FIRST_ITERATION:
-
- # present6.data[0] = previous6.data[0] = data.data[0]
- # present6.data[1] = previous6.data[1] = data.data[1]
- # present6.data[2] = previous6.data[2] = data.data[2]
- # present6.data[3] = previous6.data[3] = data.data[3]
- # present6.data[4] = previous6.data[4] = data.data[4]
- # present6.data[5] = previous6.data[5] = data.data[5]
-
-
- # FIRST_ITERATION = False
-
- # previous6.data[0] = data.data[0]
- # previous6.data[1] = data.data[1]
- # previous6.data[2] = data.data[2]
- # previous6.data[3] = data.data[3]
- # previous6.data[4] = data.data[4]
- # previous6.data[5] = data.data[5]
-
- # this line ensures that the data is always being published
- # to the thrusters.
-
- # print "Data: ", data.data
- # print data.data
- # print type(data)
- # print data.data[0]
-
-# Keyboard Control
-
-# w - forward
-# a - left
-# s - backward
- # d - right
-# t - top (towards the water surface)
-# g - ground (towards the pool bottom)
-
-string_dict = {0 : "left", 1 : "right", 2 : "backward", 3 : "forward", 4 :
- "top", 5 : "bottom", 6 : "STOP"}
-
-def create_callbacks(arg):
-
- def callback(ev=None):
-
- '''
- the argument consists the thruster number whose value has to be changed.
-
- global variable previous6 has the current values of thrusters.
-
- We will have to put the new value into present6 and
- then return from this function. This will publish the value in present6
- instead of previous6, thus changing the values given to the thrusters.
- '''
-
- print "Entered callback with thruster value ", arg
-
- global present6
- global previous6
- global THRUSTER_VALUES_CHANGED
- global pub6
-
- present6.data[0] = previous6.data[0]
- present6.data[1] = previous6.data[1]
- present6.data[2] = previous6.data[2]
- present6.data[3] = previous6.data[3]
- present6.data[4] = previous6.data[4]
- present6.data[5] = previous6.data[5]
-
-
- if arg == 0:
-
- present6.data[4] = previous6.data[4] - MIN_THRUST_INPUT
- present6.data[5] = previous6.data[5] + MIN_THRUST_INPUT
-
- if arg == 1:
-
- present6.data[4] = previous6.data[4] + MIN_THRUST_INPUT
- present6.data[5] = previous6.data[5] - MIN_THRUST_INPUT
-
- if arg == 2:
-
- present6.data[4] = previous6.data[4] - MIN_THRUST_INPUT
- present6.data[5] = previous6.data[5] - MIN_THRUST_INPUT
-
- if arg == 3:
-
- present6.data[4] = previous6.data[4] + MIN_THRUST_INPUT
- present6.data[5] = previous6.data[5] + MIN_THRUST_INPUT
-
- if arg == 4:
-
- present6.data[0] = previous6.data[0] - MIN_THRUST_INPUT
- present6.data[1] = previous6.data[1] - MIN_THRUST_INPUT
-
- if arg == 5:
-
- present6.data[0] = previous6.data[0] + MIN_THRUST_INPUT
- present6.data[1] = previous6.data[1] + MIN_THRUST_INPUT
-
- if arg == 6:
-
- present6.data[0] = 0.
- present6.data[1] = 0.
- present6.data[2] = 0.
- present6.data[3] = 0.
- present6.data[4] = 0.
- present6.data[5] = 0.
-
- THRUSTER_VALUES_CHANGED = True
-
- print "Previous values: "
- print previous6.data
-
- print "Present values: "
- print present6.data
-
- pub6.publish(present6)
-
- return callback
-
-left = Button(frame, text="left(A)", command=create_callbacks(0))
-left.grid(row=1,column=0)
-
-back = Button(frame, text="backward (S)", command=create_callbacks(2))
-back.grid(row=1,column=1)
-
-right = Button(frame, text="right (D)", command=create_callbacks(1))
-right.grid(row=1,column=2)
-
-forward = Button(frame, text="forward (W)", command=create_callbacks(3))
-forward.grid(row=0,column=1)
-
-top = Button(frame, text="top (T)", command=create_callbacks(4))
-top.grid(row=0,column=3)
-
-bottom = Button(frame, text="bottom (G)", command=create_callbacks(5))
-bottom.grid(row=1,column=3)
-
-stop = Button(frame, text="STOP (Space)", bg='red', command=create_callbacks(6))
-stop.grid(row=1,column=4)
-
-l1 = Label(frame, text="Force Values")
-l1.grid(row=0, column=5)
-
-for i in range(6):
- temp = Label(frame, textvariable=label_vars[i])
- temp.grid(row=i+1, column=5)
-
-root.bind("a", create_callbacks(0))
-root.bind("d", create_callbacks(1))
-root.bind("s", create_callbacks(2))
-root.bind("w", create_callbacks(3))
-root.bind("t", create_callbacks(4))
-root.bind("g", create_callbacks(5))
-root.bind("<space>", create_callbacks(6))
-
-frame.pack()
-
-# rospy.init_node('keyboard_control_vehicle_node', anonymous=True)
-rospy.Subscriber(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, editGui)
-pub6 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, queue_size = 10)
-
-present6 = thrusterData6Thruster()
-previous6 = thrusterData6Thruster()
-THRUSTER_VALUES_CHANGED = False
-FIRST_ITERATION = True
-MIN_THRUST_INPUT = 10.0
-prev_time = 0.
-
-root.mainloop()
diff --git a/control_system_stack/thruster_converter/src/thruster.cpp b/control_system_stack/thruster_converter/src/thruster.cpp
index 40c1e81..b9bcf69 100644
--- a/control_system_stack/thruster_converter/src/thruster.cpp
+++ b/control_system_stack/thruster_converter/src/thruster.cpp
@@ -20,8 +20,8 @@
float converter = 1.0;
uint8_t offset = 0x80;
-uint8_t max = 0xCD; //Maximum forward thrust
-uint8_t min = 0x32; //Maximum backward thrust
+uint8_t max = 0xE6; //Maximum forward thrust
+uint8_t min = 0x19; //Maximum backward thrust
kraken_msgs::seabotix _output;
void thruster4callback(const kraken_msgs::thrusterData4ThrusterConstPtr &msg)
@@ -38,11 +38,8 @@ void thruster4callback(const kraken_msgs::thrusterData4ThrusterConstPtr &msg)
for(int i = 0; i<4 ; i++ )
{
inData[i] = msg->data[i];
- ROS_INFO("indata[%d] : %f",i,inData[i]);
- store = uint8_t((converter*inData[i]>0xCD-0x80?(0xCD-0x80):converter*inData[i])+offset);
- store = uint8_t((converter*inData[i]<0x32-0x80?(0x32-0x80):converter*inData[i])+offset);
- // OFFSET - 1 FOR REVERSE SPEED (0 SPEED FOR REVERSE THRUST = 0X7f = 0X80 - 1)
- ROS_INFO("store : %d",store);
+ store = uint8_t((converter*inData[i]>0xE6-0x80?(0xE6-0x80):converter*inData[i])+offset);
+ store = uint8_t((converter*inData[i]<0x19-0x80?(0x19-0x80):converter*inData[i])+offset);
if (store > max)
{
@@ -62,18 +59,6 @@ void thruster4callback(const kraken_msgs::thrusterData4ThrusterConstPtr &msg)
void thruster6callback(const kraken_msgs::thrusterData6ThrusterConstPtr msg)
{
-
- /*
- HOTFIX by Aniket in May, 2015
-
- Due to thruster remapping, the if else constructs
- have to be introduced here, so as to ensure that
- the other packages that depend on this convention:
-
- thruster data array[4] -> surge left
- thruster data array[5] -> surge right
-
- */
float inData[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
uint8_t store;
@@ -87,8 +72,8 @@ void thruster6callback(const kraken_msgs::thrusterData6ThrusterConstPtr msg)
{
inData[i] = msg->data[i];
ROS_INFO("indata[%d] : %f",i,inData[i]);
- store = uint8_t((converter*inData[i]>(0xCD-0x80)?(0xCD):converter*inData[i]+offset));
- store = uint8_t((converter*inData[i]<(0x32-0x7F)?(0x32):converter*inData[i]+offset-1));
+ store = uint8_t((converter*inData[i]>(0xE6-0x80)?(0xE6):converter*inData[i]+0x80));
+ store = uint8_t((converter*inData[i]<(0x19-0x80)?(0x19):converter*inData[i]+0x80));
ROS_INFO("store : %d",store);
if (store > max)
@@ -101,26 +86,7 @@ void thruster6callback(const kraken_msgs::thrusterData6ThrusterConstPtr msg)
store = min;
}
- if(i == 4)
-
- {
- _output.data[1] = store;
- }
-
- else
-
- if (i == 5)
-
- {
- _output.data[4] = store;
- }
-
- else
-
- {
- _output.data[i] = store;
- }
-
+ _output.data[i] = store;
}
}
@@ -131,9 +97,9 @@ int main(int argc,char** argv)
ros::init(argc ,argv, "seabotixConverter");
ros::NodeHandle n;
- ros::Subscriber _sub4 = n.subscribe<kraken_msgs::thrusterData4Thruster>(topics::CONTROL_PID_THRUSTER4,1,thruster4callback);
- ros::Subscriber _sub6 = n.subscribe<kraken_msgs::thrusterData6Thruster>(topics::CONTROL_PID_THRUSTER6,1,thruster6callback);
- ros::Publisher _pub = n.advertise<kraken_msgs::seabotix>(topics::CONTROL_SEABOTIX,1);
+ ros::Subscriber _sub4 = n.subscribe<kraken_msgs::thrusterData4Thruster>(topics::CONTROL_PID_THRUSTER4,2,thruster4callback);
+ ros::Subscriber _sub6 = n.subscribe<kraken_msgs::thrusterData6Thruster>(topics::CONTROL_PID_THRUSTER6,2,thruster6callback);
+ ros::Publisher _pub = n.advertise<kraken_msgs::seabotix>(topics::CONTROL_SEABOTIX,2);
// Serial arduino;
diff --git a/gui_stack/gui_controller_loader/cfg/cpp/gui_controller_loader/paramsConfig.h b/gui_stack/gui_controller_loader/cfg/cpp/gui_controller_loader/paramsConfig.h
new file mode 100644
index 0000000..22f385f
--- /dev/null
+++ b/gui_stack/gui_controller_loader/cfg/cpp/gui_controller_loader/paramsConfig.h
@@ -0,0 +1,766 @@
+//#line 2 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template"
+// *********************************************************
+//
+// File autogenerated for the gui_controller_loader package
+// by the dynamic_reconfigure package.
+// Please do not edit.
+//
+// ********************************************************/
+
+/***********************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ ***********************************************************/
+
+// Author: Blaise Gassend
+
+
+#ifndef __gui_controller_loader__PARAMSCONFIG_H__
+#define __gui_controller_loader__PARAMSCONFIG_H__
+
+#include <dynamic_reconfigure/config_tools.h>
+#include <limits>
+#include <ros/node_handle.h>
+#include <dynamic_reconfigure/ConfigDescription.h>
+#include <dynamic_reconfigure/ParamDescription.h>
+#include <dynamic_reconfigure/Group.h>
+#include <dynamic_reconfigure/config_init_mutex.h>
+#include <boost/any.hpp>
+
+namespace gui_controller_loader
+{
+class paramsConfigStatics;
+
+class paramsConfig
+{
+public:
+ class AbstractParamDescription : public dynamic_reconfigure::ParamDescription
+ {
+ public:
+ AbstractParamDescription(std::string n, std::string t, uint32_t l,
+ std::string d, std::string e)
+ {
+ name = n;
+ type = t;
+ level = l;
+ description = d;
+ edit_method = e;
+ }
+
+ virtual void clamp(paramsConfig &config, const paramsConfig &max, const paramsConfig &min) const = 0;
+ virtual void calcLevel(uint32_t &level, const paramsConfig &config1, const paramsConfig &config2) const = 0;
+ virtual void fromServer(const ros::NodeHandle &nh, paramsConfig &config) const = 0;
+ virtual void toServer(const ros::NodeHandle &nh, const paramsConfig &config) const = 0;
+ virtual bool fromMessage(const dynamic_reconfigure::Config &msg, paramsConfig &config) const = 0;
+ virtual void toMessage(dynamic_reconfigure::Config &msg, const paramsConfig &config) const = 0;
+ virtual void getValue(const paramsConfig &config, boost::any &val) const = 0;
+ };
+
+ typedef boost::shared_ptr<AbstractParamDescription> AbstractParamDescriptionPtr;
+ typedef boost::shared_ptr<const AbstractParamDescription> AbstractParamDescriptionConstPtr;
+
+ template <class T>
+ class ParamDescription : public AbstractParamDescription
+ {
+ public:
+ ParamDescription(std::string name, std::string type, uint32_t level,
+ std::string description, std::string edit_method, T paramsConfig::* f) :
+ AbstractParamDescription(name, type, level, description, edit_method),
+ field(f)
+ {}
+
+ T (paramsConfig::* field);
+
+ virtual void clamp(paramsConfig &config, const paramsConfig &max, const paramsConfig &min) const
+ {
+ if (config.*field > max.*field)
+ {
+ config.*field = max.*field;
+ }
+
+ if (config.*field < min.*field)
+ {
+ config.*field = min.*field;
+ }
+ }
+
+ virtual void calcLevel(uint32_t &comb_level, const paramsConfig &config1, const paramsConfig &config2) const
+ {
+ if (config1.*field != config2.*field)
+ {
+ comb_level |= level;
+ }
+ }
+
+ virtual void fromServer(const ros::NodeHandle &nh, paramsConfig &config) const
+ {
+ nh.getParam(name, config.*field);
+ }
+
+ virtual void toServer(const ros::NodeHandle &nh, const paramsConfig &config) const
+ {
+ nh.setParam(name, config.*field);
+ }
+
+ virtual bool fromMessage(const dynamic_reconfigure::Config &msg, paramsConfig &config) const
+ {
+ return dynamic_reconfigure::ConfigTools::getParameter(msg, name, config.*field);
+ }
+
+ virtual void toMessage(dynamic_reconfigure::Config &msg, const paramsConfig &config) const
+ {
+ dynamic_reconfigure::ConfigTools::appendParameter(msg, name, config.*field);
+ }
+
+ virtual void getValue(const paramsConfig &config, boost::any &val) const
+ {
+ val = config.*field;
+ }
+ };
+
+ class AbstractGroupDescription : public dynamic_reconfigure::Group
+ {
+ public:
+ AbstractGroupDescription(std::string n, std::string t, int p, int i, bool s)
+ {
+ name = n;
+ type = t;
+ parent = p;
+ state = s;
+ id = i;
+ }
+
+ std::vector<AbstractParamDescriptionConstPtr> abstract_parameters;
+ bool state;
+
+ virtual void toMessage(dynamic_reconfigure::Config &msg, const boost::any &config) const = 0;
+ virtual bool fromMessage(const dynamic_reconfigure::Config &msg, boost::any &config) const =0;
+ virtual void updateParams(boost::any &cfg, paramsConfig &top) const= 0;
+ virtual void setInitialState(boost::any &cfg) const = 0;
+
+
+ void convertParams()
+ {
+ for(std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = abstract_parameters.begin(); i != abstract_parameters.end(); ++i)
+ {
+ parameters.push_back(dynamic_reconfigure::ParamDescription(**i));
+ }
+ }
+ };
+
+ typedef boost::shared_ptr<AbstractGroupDescription> AbstractGroupDescriptionPtr;
+ typedef boost::shared_ptr<const AbstractGroupDescription> AbstractGroupDescriptionConstPtr;
+
+ template<class T, class PT>
+ class GroupDescription : public AbstractGroupDescription
+ {
+ public:
+ GroupDescription(std::string name, std::string type, int parent, int id, bool s, T PT::* f) : AbstractGroupDescription(name, type, parent, id, s), field(f)
+ {
+ }
+
+ GroupDescription(const GroupDescription<T, PT>& g): AbstractGroupDescription(g.name, g.type, g.parent, g.id, g.state), field(g.field), groups(g.groups)
+ {
+ parameters = g.parameters;
+ abstract_parameters = g.abstract_parameters;
+ }
+
+ virtual bool fromMessage(const dynamic_reconfigure::Config &msg, boost::any &cfg) const
+ {
+ PT* config = boost::any_cast<PT*>(cfg);
+
+ if(!dynamic_reconfigure::ConfigTools::getGroupState(msg, name, (*config).*field))
+ {
+ return false;
+ }
+
+ for(std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = groups.begin(); i != groups.end(); ++i)
+ {
+ boost::any n = &((*config).*field);
+
+ if(!(*i)->fromMessage(msg, n))
+ {
+ return false;
+ }
+ }
+
+ return true;
+ }
+
+ virtual void setInitialState(boost::any &cfg) const
+ {
+ PT* config = boost::any_cast<PT*>(cfg);
+ T* group = &((*config).*field);
+ group->state = state;
+
+ for(std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = groups.begin(); i != groups.end(); ++i)
+ {
+ boost::any n = boost::any(&((*config).*field));
+ (*i)->setInitialState(n);
+ }
+
+ }
+
+ virtual void updateParams(boost::any &cfg, paramsConfig &top) const
+ {
+ PT* config = boost::any_cast<PT*>(cfg);
+
+ T* f = &((*config).*field);
+ f->setParams(top, abstract_parameters);
+
+ for(std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = groups.begin(); i != groups.end(); ++i)
+ {
+ boost::any n = &((*config).*field);
+ (*i)->updateParams(n, top);
+ }
+ }
+
+ virtual void toMessage(dynamic_reconfigure::Config &msg, const boost::any &cfg) const
+ {
+ const PT config = boost::any_cast<PT>(cfg);
+ dynamic_reconfigure::ConfigTools::appendGroup<T>(msg, name, id, parent, config.*field);
+
+ for(std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = groups.begin(); i != groups.end(); ++i)
+ {
+ (*i)->toMessage(msg, config.*field);
+ }
+ }
+
+ T (PT::* field);
+ std::vector<paramsConfig::AbstractGroupDescriptionConstPtr> groups;
+ };
+
+ class DEFAULT
+ {
+ public:
+ DEFAULT()
+ {
+ state = true;
+ name = "Default";
+ }
+
+ void setParams(paramsConfig &config, const std::vector<AbstractParamDescriptionConstPtr> params)
+ {
+ for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator _i = params.begin(); _i != params.end(); ++_i)
+ {
+ boost::any val;
+ (*_i)->getValue(config, val);
+
+ if("off_yaw"==(*_i)->name)
+ {
+ off_yaw = boost::any_cast<double>(val);
+ }
+
+ if("Kp_yaw"==(*_i)->name)
+ {
+ Kp_yaw = boost::any_cast<double>(val);
+ }
+
+ if("Kd_yaw"==(*_i)->name)
+ {
+ Kd_yaw = boost::any_cast<double>(val);
+ }
+
+ if("Ki_yaw"==(*_i)->name)
+ {
+ Ki_yaw = boost::any_cast<double>(val);
+ }
+
+ if("off_dep_bot"==(*_i)->name)
+ {
+ off_dep_bot = boost::any_cast<double>(val);
+ }
+
+ if("Kib_depth"==(*_i)->name)
+ {
+ Kib_depth = boost::any_cast<double>(val);
+ }
+
+ if("Kpb_depth"==(*_i)->name)
+ {
+ Kpb_depth = boost::any_cast<double>(val);
+ }
+
+ if("Kdb_depth"==(*_i)->name)
+ {
+ Kdb_depth = boost::any_cast<double>(val);
+ }
+
+ if("off_dep_top"==(*_i)->name)
+ {
+ off_dep_top = boost::any_cast<double>(val);
+ }
+
+ if("Kpt_depth"==(*_i)->name)
+ {
+ Kpt_depth = boost::any_cast<double>(val);
+ }
+
+ if("Kdt_depth"==(*_i)->name)
+ {
+ Kdt_depth = boost::any_cast<double>(val);
+ }
+
+ if("Kit_depth"==(*_i)->name)
+ {
+ Kit_depth = boost::any_cast<double>(val);
+ }
+ }
+ }
+
+ double off_yaw;
+ double Kp_yaw;
+ double Kd_yaw;
+ double Ki_yaw;
+ double off_dep_bot;
+ double Kib_depth;
+ double Kpb_depth;
+ double Kdb_depth;
+ double off_dep_top;
+ double Kpt_depth;
+ double Kdt_depth;
+ double Kit_depth;
+
+ bool state;
+ std::string name;
+
+
+ } groups;
+
+
+
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ double off_yaw;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ double Kp_yaw;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ double Kd_yaw;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ double Ki_yaw;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ double off_dep_bot;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ double Kib_depth;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ double Kpb_depth;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ double Kdb_depth;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ double off_dep_top;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ double Kpt_depth;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ double Kdt_depth;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ double Kit_depth;
+//#line 255 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template"
+
+ bool __fromMessage__(dynamic_reconfigure::Config &msg)
+ {
+ const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
+ const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__ = __getGroupDescriptions__();
+
+ int count = 0;
+
+ for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i)
+ if ((*i)->fromMessage(msg, *this))
+ {
+ count++;
+ }
+
+ for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i ++)
+ {
+ if ((*i)->id == 0)
+ {
+ boost::any n = boost::any(this);
+ (*i)->updateParams(n, *this);
+ (*i)->fromMessage(msg, n);
+ }
+ }
+
+ if (count != dynamic_reconfigure::ConfigTools::size(msg))
+ {
+ ROS_ERROR("paramsConfig::__fromMessage__ called with an unexpected parameter.");
+ ROS_ERROR("Booleans:");
+
+ for (unsigned int i = 0; i < msg.bools.size(); i++)
+ {
+ ROS_ERROR(" %s", msg.bools[i].name.c_str());
+ }
+
+ ROS_ERROR("Integers:");
+
+ for (unsigned int i = 0; i < msg.ints.size(); i++)
+ {
+ ROS_ERROR(" %s", msg.ints[i].name.c_str());
+ }
+
+ ROS_ERROR("Doubles:");
+
+ for (unsigned int i = 0; i < msg.doubles.size(); i++)
+ {
+ ROS_ERROR(" %s", msg.doubles[i].name.c_str());
+ }
+
+ ROS_ERROR("Strings:");
+
+ for (unsigned int i = 0; i < msg.strs.size(); i++)
+ {
+ ROS_ERROR(" %s", msg.strs[i].name.c_str());
+ }
+
+ // @todo Check that there are no duplicates. Make this error more
+ // explicit.
+ return false;
+ }
+
+ return true;
+ }
+
+ // This version of __toMessage__ is used during initialization of
+ // statics when __getParamDescriptions__ can't be called yet.
+ void __toMessage__(dynamic_reconfigure::Config &msg, const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__, const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__) const
+ {
+ dynamic_reconfigure::ConfigTools::clear(msg);
+
+ for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i)
+ {
+ (*i)->toMessage(msg, *this);
+ }
+
+ for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); ++i)
+ {
+ if((*i)->id == 0)
+ {
+ (*i)->toMessage(msg, *this);
+ }
+ }
+ }
+
+ void __toMessage__(dynamic_reconfigure::Config &msg) const
+ {
+ const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
+ const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__ = __getGroupDescriptions__();
+ __toMessage__(msg, __param_descriptions__, __group_descriptions__);
+ }
+
+ void __toServer__(const ros::NodeHandle &nh) const
+ {
+ const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
+
+ for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i)
+ {
+ (*i)->toServer(nh, *this);
+ }
+ }
+
+ void __fromServer__(const ros::NodeHandle &nh)
+ {
+ static bool setup=false;
+
+ const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
+
+ for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i)
+ {
+ (*i)->fromServer(nh, *this);
+ }
+
+ const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__ = __getGroupDescriptions__();
+
+ for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i++)
+ {
+ if (!setup && (*i)->id == 0)
+ {
+ setup = true;
+ boost::any n = boost::any(this);
+ (*i)->setInitialState(n);
+ }
+ }
+ }
+
+ void __clamp__()
+ {
+ const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
+ const paramsConfig &__max__ = __getMax__();
+ const paramsConfig &__min__ = __getMin__();
+
+ for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i)
+ {
+ (*i)->clamp(*this, __max__, __min__);
+ }
+ }
+
+ uint32_t __level__(const paramsConfig &config) const
+ {
+ const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
+ uint32_t level = 0;
+
+ for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i)
+ {
+ (*i)->calcLevel(level, config, *this);
+ }
+
+ return level;
+ }
+
+ static const dynamic_reconfigure::ConfigDescription &__getDescriptionMessage__();
+ static const paramsConfig &__getDefault__();
+ static const paramsConfig &__getMax__();
+ static const paramsConfig &__getMin__();
+ static const std::vector<AbstractParamDescriptionConstPtr> &__getParamDescriptions__();
+ static const std::vector<AbstractGroupDescriptionConstPtr> &__getGroupDescriptions__();
+
+private:
+ static const paramsConfigStatics *__get_statics__();
+};
+
+template <> // Max and min are ignored for strings.
+inline void paramsConfig::ParamDescription<std::string>::clamp(paramsConfig &config, const paramsConfig &max, const paramsConfig &min) const
+{
+ return;
+}
+
+class paramsConfigStatics
+{
+ friend class paramsConfig;
+
+ paramsConfigStatics()
+ {
+ paramsConfig::GroupDescription<paramsConfig::DEFAULT, paramsConfig> Default("Default", "", 0, 0, true, &paramsConfig::groups);
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __min__.off_yaw = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __max__.off_yaw = 100.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __default__.off_yaw = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ Default.abstract_parameters.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("off_yaw", "double", 0, "Kp value", "", &paramsConfig::off_yaw)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __param_descriptions__.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("off_yaw", "double", 0, "Kp value", "", &paramsConfig::off_yaw)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __min__.Kp_yaw = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __max__.Kp_yaw = 100.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __default__.Kp_yaw = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ Default.abstract_parameters.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kp_yaw", "double", 0, "Kp value", "", &paramsConfig::Kp_yaw)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __param_descriptions__.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kp_yaw", "double", 0, "Kp value", "", &paramsConfig::Kp_yaw)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __min__.Kd_yaw = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __max__.Kd_yaw = 100.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __default__.Kd_yaw = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ Default.abstract_parameters.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kd_yaw", "double", 0, "A double parameter", "", &paramsConfig::Kd_yaw)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __param_descriptions__.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kd_yaw", "double", 0, "A double parameter", "", &paramsConfig::Kd_yaw)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __min__.Ki_yaw = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __max__.Ki_yaw = 100.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __default__.Ki_yaw = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ Default.abstract_parameters.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Ki_yaw", "double", 0, "A double parameter", "", &paramsConfig::Ki_yaw)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __param_descriptions__.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Ki_yaw", "double", 0, "A double parameter", "", &paramsConfig::Ki_yaw)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __min__.off_dep_bot = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __max__.off_dep_bot = 100.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __default__.off_dep_bot = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ Default.abstract_parameters.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("off_dep_bot", "double", 0, "Kp value", "", &paramsConfig::off_dep_bot)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __param_descriptions__.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("off_dep_bot", "double", 0, "Kp value", "", &paramsConfig::off_dep_bot)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __min__.Kib_depth = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __max__.Kib_depth = 100.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __default__.Kib_depth = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ Default.abstract_parameters.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kib_depth", "double", 0, "A double parameter", "", &paramsConfig::Kib_depth)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __param_descriptions__.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kib_depth", "double", 0, "A double parameter", "", &paramsConfig::Kib_depth)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __min__.Kpb_depth = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __max__.Kpb_depth = 100.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __default__.Kpb_depth = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ Default.abstract_parameters.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kpb_depth", "double", 0, "Kp value", "", &paramsConfig::Kpb_depth)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __param_descriptions__.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kpb_depth", "double", 0, "Kp value", "", &paramsConfig::Kpb_depth)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __min__.Kdb_depth = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __max__.Kdb_depth = 100.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __default__.Kdb_depth = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ Default.abstract_parameters.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kdb_depth", "double", 0, "A double parameter", "", &paramsConfig::Kdb_depth)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __param_descriptions__.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kdb_depth", "double", 0, "A double parameter", "", &paramsConfig::Kdb_depth)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __min__.off_dep_top = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __max__.off_dep_top = 100.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __default__.off_dep_top = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ Default.abstract_parameters.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("off_dep_top", "double", 0, "Kp value", "", &paramsConfig::off_dep_top)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __param_descriptions__.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("off_dep_top", "double", 0, "Kp value", "", &paramsConfig::off_dep_top)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __min__.Kpt_depth = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __max__.Kpt_depth = 100.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __default__.Kpt_depth = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ Default.abstract_parameters.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kpt_depth", "double", 0, "Kp value", "", &paramsConfig::Kpt_depth)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __param_descriptions__.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kpt_depth", "double", 0, "Kp value", "", &paramsConfig::Kpt_depth)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __min__.Kdt_depth = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __max__.Kdt_depth = 100.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __default__.Kdt_depth = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ Default.abstract_parameters.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kdt_depth", "double", 0, "A double parameter", "", &paramsConfig::Kdt_depth)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __param_descriptions__.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kdt_depth", "double", 0, "A double parameter", "", &paramsConfig::Kdt_depth)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __min__.Kit_depth = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __max__.Kit_depth = 100.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __default__.Kit_depth = 0.0;
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ Default.abstract_parameters.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kit_depth", "double", 0, "A double parameter", "", &paramsConfig::Kit_depth)));
+//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __param_descriptions__.push_back(paramsConfig::AbstractParamDescriptionConstPtr(new paramsConfig::ParamDescription<double>("Kit_depth", "double", 0, "A double parameter", "", &paramsConfig::Kit_depth)));
+//#line 233 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ Default.convertParams();
+//#line 233 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
+ __group_descriptions__.push_back(paramsConfig::AbstractGroupDescriptionConstPtr(new paramsConfig::GroupDescription<paramsConfig::DEFAULT, paramsConfig>(Default)));
+//#line 390 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template"
+
+ for (std::vector<paramsConfig::AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); ++i)
+ {
+ __description_message__.groups.push_back(**i);
+ }
+
+ __max__.__toMessage__(__description_message__.max, __param_descriptions__, __group_descriptions__);
+ __min__.__toMessage__(__description_message__.min, __param_descriptions__, __group_descriptions__);
+ __default__.__toMessage__(__description_message__.dflt, __param_descriptions__, __group_descriptions__);
+ }
+ std::vector<paramsConfig::AbstractParamDescriptionConstPtr> __param_descriptions__;
+ std::vector<paramsConfig::AbstractGroupDescriptionConstPtr> __group_descriptions__;
+ paramsConfig __max__;
+ paramsConfig __min__;
+ paramsConfig __default__;
+ dynamic_reconfigure::ConfigDescription __description_message__;
+
+ static const paramsConfigStatics *get_instance()
+ {
+ // Split this off in a separate function because I know that
+ // instance will get initialized the first time get_instance is
+ // called, and I am guaranteeing that get_instance gets called at
+ // most once.
+ static paramsConfigStatics instance;
+ return &instance;
+ }
+};
+
+inline const dynamic_reconfigure::ConfigDescription &paramsConfig::__getDescriptionMessage__()
+{
+ return __get_statics__()->__description_message__;
+}
+
+inline const paramsConfig &paramsConfig::__getDefault__()
+{
+ return __get_statics__()->__default__;
+}
+
+inline const paramsConfig &paramsConfig::__getMax__()
+{
+ return __get_statics__()->__max__;
+}
+
+inline const paramsConfig &paramsConfig::__getMin__()
+{
+ return __get_statics__()->__min__;
+}
+
+inline const std::vector<paramsConfig::AbstractParamDescriptionConstPtr> &paramsConfig::__getParamDescriptions__()
+{
+ return __get_statics__()->__param_descriptions__;
+}
+
+inline const std::vector<paramsConfig::AbstractGroupDescriptionConstPtr> &paramsConfig::__getGroupDescriptions__()
+{
+ return __get_statics__()->__group_descriptions__;
+}
+
+inline const paramsConfigStatics *paramsConfig::__get_statics__()
+{
+ const static paramsConfigStatics *statics;
+
+ if (statics) // Common case
+ {
+ return statics;
+ }
+
+ boost::mutex::scoped_lock lock(dynamic_reconfigure::__init_mutex__);
+
+ if (statics) // In case we lost a race.
+ {
+ return statics;
+ }
+
+ statics = paramsConfigStatics::get_instance();
+
+ return statics;
+}
+
+
+}
+
+#endif // __PARAMSRECONFIGURATOR_H__
diff --git a/gui_stack/gui_controller_loader/src/gui_controller_loader/cfg/paramsConfig.py b/gui_stack/gui_controller_loader/src/gui_controller_loader/cfg/paramsConfig.py
new file mode 100644
index 0000000..8e52b1e
--- /dev/null
+++ b/gui_stack/gui_controller_loader/src/gui_controller_loader/cfg/paramsConfig.py
@@ -0,0 +1,70 @@
+## *********************************************************
+##
+## File autogenerated for the gui_controller_loader package
+## by the dynamic_reconfigure package.
+## Please do not edit.
+##
+## ********************************************************/
+
+##**********************************************************
+## Software License Agreement (BSD License)
+##
+## Copyright (c) 2008, Willow Garage, Inc.
+## All rights reserved.
+##
+## Redistribution and use in source and binary forms, with or without
+## modification, are permitted provided that the following conditions
+## are met:
+##
+## * Redistributions of source code must retain the above copyright
+## notice, this list of conditions and the following disclaimer.
+## * Redistributions in binary form must reproduce the above
+## copyright notice, this list of conditions and the following
+## disclaimer in the documentation and/or other materials provided
+## with the distribution.
+## * Neither the name of the Willow Garage nor the names of its
+## contributors may be used to endorse or promote products derived
+## from this software without specific prior written permission.
+##
+## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+## "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+## LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+## FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+## COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+## INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+## BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+## LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+## CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+## LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+## ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+## POSSIBILITY OF SUCH DAMAGE.
+##**********************************************************/
+
+from dynamic_reconfigure.encoding import extract_params
+
+inf = float('inf')
+
+config_description = {'upper': 'DEFAULT', 'lower': 'groups', 'srcline': 233, 'name': 'Default', 'parent': 0, 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'cstate': 'true', 'parentname': 'Default', 'class': 'DEFAULT', 'field': 'default', 'state': True, 'parentclass': '', 'groups': [], 'parameters': [{'srcline': 259, 'description': 'Kp value', 'max': 100.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'off_yaw', 'edit_method': '', 'default': 0.0, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'Kp value', 'max': 100.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'Kp_yaw', 'edit_method': '', 'default': 0.0, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'A double parameter', 'max': 100.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'Kd_yaw', 'edit_method': '', 'default': 0.0, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'A double parameter', 'max': 100.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'Ki_yaw', 'edit_method': '', 'default': 0.0, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'Kp value', 'max': 100.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'off_dep_bot', 'edit_method': '', 'default': 0.0, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'A double parameter', 'max': 100.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'Kib_depth', 'edit_method': '', 'default': 0.0, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'Kp value', 'max': 100.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'Kpb_depth', 'edit_method': '', 'default': 0.0, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'A double parameter', 'max': 100.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'Kdb_depth', 'edit_method': '', 'default': 0.0, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'Kp value', 'max': 100.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'off_dep_top', 'edit_method': '', 'default': 0.0, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'Kp value', 'max': 100.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'Kpt_depth', 'edit_method': '', 'default': 0.0, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'A double parameter', 'max': 100.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'Kdt_depth', 'edit_method': '', 'default': 0.0, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'A double parameter', 'max': 100.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'Kit_depth', 'edit_method': '', 'default': 0.0, 'level': 0, 'min': 0.0, 'type': 'double'}], 'type': '', 'id': 0}
+
+min = {}
+max = {}
+defaults = {}
+level = {}
+type = {}
+all_level = 0
+
+#def extract_params(config):
+# params = []
+# params.extend(config['parameters'])
+# for group in config['groups']:
+# params.extend(extract_params(group))
+# return params
+
+for param in extract_params(config_description):
+ min[param['name']] = param['min']
+ max[param['name']] = param['max']
+ defaults[param['name']] = param['default']
+ level[param['name']] = param['level']
+ type[param['name']] = param['type']
+ all_level = all_level | param['level']
+
diff --git a/hooks/formatAll.sh b/hooks/formatAll.sh
new file mode 100644
index 0000000..47b2e45
--- /dev/null
+++ b/hooks/formatAll.sh
@@ -0,0 +1,12 @@
+extensions=('*.cpp' '*.hpp' '*.h')
+
+for j in "${extensions[@]}"; do
+ find ./ -name $j >> .tmp.txt
+ echo $j
+done;
+
+for i in `cat .tmp.txt`; do
+ astyle -A1 -yej -f -S $i
+done;
+
+rm .tmp.txt
diff --git a/mission_planner_stack/pose_server/src/DeadReckoning.cpp b/mission_planner_stack/pose_server/src/DeadReckoning.cpp
index d71e70d..83f72ea 100644
--- a/mission_planner_stack/pose_server/src/DeadReckoning.cpp
+++ b/mission_planner_stack/pose_server/src/DeadReckoning.cpp
@@ -1,6 +1,5 @@
#include <pose_server/DeadReckoning.h>
-
namespace kraken_core
{
DeadReckoning::DeadReckoning(int size, float time):Estimator(size,time)
@@ -112,12 +111,17 @@ void DeadReckoning::updateCurrentVelocity(kraken_msgs::dvlData &dvl_data)
_data_world_next[_az] = (_data_world_next[_vz] - _data[_vz])/_time;
}
+// updating the current position on the basis of the data from the depth sensor
+// we don't apply the equation in the z axis, we use the data from the
+// sensor.
void DeadReckoning::updateCurrentPosition(kraken_msgs::depthData & depth)
{
float* _data_world_next = _next_pose_world.getData();
boost::circular_buffer<KrakenPose>::iterator start = _prev_states_world.end()-1;
float* _data = (*start).getData();
// Calculate world positions
+ // s = u*t + 0.5 * a * t^2
+ // s = (u + 0.5 * a * t) * t
_data_world_next[_px] = _data[_px]+(_data[_vx]+_data[_ax]*_time/2.0)*_time;
_data_world_next[_py] = _data[_py]+(_data[_vy]+_data[_ay]*_time/2.0)*_time;
_data_world_next[_pz] = depth.depth;
@@ -133,12 +137,15 @@ void DeadReckoning::updateCurrentPosition(kraken_msgs::depthData & depth)
_prev_states_body.push_back(_next_pose_body);
}
+// update the current position, by applying the second equation in all
+// three axes.
void DeadReckoning::updateCurrentPosition()
{
float* _data_world_next = _next_pose_world.getData();
boost::circular_buffer<KrakenPose>::iterator start = _prev_states_world.end()-1;
float* _data = (*start).getData();
// Calculate world positions
+
_data_world_next[_px] = _data[_px]+(_data[_vx]+_data[_ax]*_time/2.0)*_time;
_data_world_next[_py] = _data[_py]+(_data[_vy]+_data[_ay]*_time/2.0)*_time;
_data_world_next[_pz] = _data[_pz]+(_data[_vz]+_data[_az]*_time/2.0)*_time;
diff --git a/mission_planner_stack/pose_server/src/KalmanEstimator.cpp b/mission_planner_stack/pose_server/src/KalmanEstimator.cpp
index a0d1105..4814049 100644
--- a/mission_planner_stack/pose_server/src/KalmanEstimator.cpp
+++ b/mission_planner_stack/pose_server/src/KalmanEstimator.cpp
@@ -6,7 +6,8 @@ KalmanEstimator::KalmanEstimator(int size, float time):Estimator(size,time),_isS
{
- //defining all the 4 matrices;
+ // defining all the 4 matrices;
+
_Hmatrix<<0,0,1,0,
0,0,0,1;
@@ -105,21 +106,19 @@ void KalmanEstimator::kalmanMeasurementUpdate(double vx, double vy)
Vector4d X;
X=getStateMatrix();
-
y=Z-_Hmatrix*X;
+
Matrix2d S=_Hmatrix*_Pmatrix*_Hmatrix.transpose()+ _Rmatrix;
//assuming S is invertible
Matrix<double,4,2> K=_Pmatrix*_Hmatrix.transpose()*S.inverse();
+ Vector4d temp=X+K*y; // new position
- Vector4d temp=X+K*y;
kalmanUpdateStateFromMatrix(temp);
- _Pmatrix=(Matrix4d::Identity()-K*_Hmatrix)*_Pmatrix;
-}
-
-
+ _Pmatrix=(Matrix4d::Identity()-K*_Hmatrix)*_Pmatrix; // new covariance
+}
void KalmanEstimator::kalmanUpdateStateFromMatrix(const Vector4d &stateVector)
{
@@ -130,6 +129,7 @@ void KalmanEstimator::kalmanUpdateStateFromMatrix(const Vector4d &stateVector)
current_state_array[kraken_core::_vy]=stateVector(3);
}
+// updating the state on the basis of the data from IMU
void KalmanEstimator::updateState(kraken_msgs::imuData &imu)
{
//should be called at first for updating all the required imu values
@@ -165,6 +165,7 @@ void KalmanEstimator::updateState(kraken_msgs::imuData &imu)
accelerationToWorld();
}
+// updateState on the basis of data from the Depth Sensor
void KalmanEstimator::updateState(kraken_msgs::depthData &depth_data_msg)
{
float *world_data=_next_pose_world.getData();
@@ -172,6 +173,7 @@ void KalmanEstimator::updateState(kraken_msgs::depthData &depth_data_msg)
world_data[kraken_core::_pz]=depth_data_msg.depth;
}
+// updateState on the basis of data from DVL
void KalmanEstimator::updateState(kraken_msgs::dvlData& dvl_data)
{
float* body_data=_next_pose_body.getData();
@@ -183,15 +185,12 @@ void KalmanEstimator::updateState(kraken_msgs::dvlData& dvl_data)
velocityToWorld();
}
-
-
void KalmanEstimator::accelerationToWorld()
{
//gets acceleration in world frame in _next_pose_world
float* world_data=_next_pose_world.getData();
float *body_data=_next_pose_body.getData();
-
float ax=body_data[kraken_core::_ax];
float ay=body_data[kraken_core::_ay];
float az=body_data[kraken_core::_az];
@@ -199,7 +198,6 @@ void KalmanEstimator::accelerationToWorld()
float p=body_data[kraken_core::_pitch];
float y=body_data[kraken_core::_yaw];
-
Matrix3d RWB;
RWB<<cos(p)*cos(y), sin(r)*sin(p)*cos(y)-cos(r)*sin(y), cos(r)*sin(p)*cos(y)+sin(r)*sin(y),
cos(p)*sin(y), sin(r)*sin(p)*sin(y)+cos(r)*cos(y), cos(r)*sin(p)*sin(y)-sin(r)*cos(y),
diff --git a/mission_planner_stack/pose_server/src/PoseServer.cpp b/mission_planner_stack/pose_server/src/PoseServer.cpp
index 81e7bcf..e89ba58 100644
--- a/mission_planner_stack/pose_server/src/PoseServer.cpp
+++ b/mission_planner_stack/pose_server/src/PoseServer.cpp
@@ -9,7 +9,7 @@ PoseServer::PoseServer(Estimator *estimator):_estimator(estimator)
ros::NodeHandle n;
_imu_sub = n.subscribe<kraken_msgs::imuData>(topics::SENSOR_IMU,1,&PoseServer::imuCallBack,this);
//_imu = n.subscribe<sensor_msgs::Imu>("/kraken/imuData",2,&PoseServer::imuCallBack,this);
-// _depth_sub = n.subscribe<underwater_sensor_msgs::Pressure>(topics::SENSOR_DEPTH,1,&PoseServer::depthCallBack,this);
+ //_depth_sub = n.subscribe<underwater_sensor_msgs::Pressure>(topics::SENSOR_DEPTH,1,&PoseServer::depthCallBack,this);
//_depth = n.subscribe<kraken_msgs::depthData>("/kraken/depth",2,&PoseServer::depthCallBack,this);
_dvl_sub = n.subscribe<kraken_msgs::dvlData>(topics::SENSOR_DVL,2,&PoseServer::dvlCallBack,this);
_pose_pub = n.advertise<kraken_msgs::krakenPose>(topics::NAV_POSE_ESTIMATED,1);
diff --git a/mission_planner_stack/pose_server_python/manifest.xml b/mission_planner_stack/pose_server_python/manifest.xml
index 7b017b1..c7731ce 100644
--- a/mission_planner_stack/pose_server_python/manifest.xml
+++ b/mission_planner_stack/pose_server_python/manifest.xml
@@ -10,8 +10,7 @@
<url>http://ros.org/wiki/pose_server_python</url>
<depend package="rospy"/>
<depend package="resources"/>
+ <depend package="kraken_msgs"/>
<depend package="std_msgs"/>
</package>
-
-
diff --git a/mission_planner_stack/pose_server_python/src/kal5.py b/mission_planner_stack/pose_server_python/src/kal5.py
deleted file mode 100644
index e117984..0000000
--- a/mission_planner_stack/pose_server_python/src/kal5.py
+++ /dev/null
@@ -1,243 +0,0 @@
-# Fill in the matrices P, F, H, R and I at the bottom
-#
-# This question requires NO CODING, just fill in the
-# matrices where indicated. Please do not delete or modify
-# any provided code OR comments. Good luck!
-
-from math import *
-
-class matrix:
-
- # implements basic operations of a matrix class
-
- def __init__(self, value):
- self.value = value
- self.dimx = len(value)
- self.dimy = len(value[0])
- if value == [[]]:
- self.dimx = 0
-
- def zero(self, dimx, dimy):
- # check if valid dimensions
- if dimx < 1 or dimy < 1:
- raise ValueError, "Invalid size of matrix"
- else:
- self.dimx = dimx
- self.dimy = dimy
- self.value = [[0 for row in range(dimy)] for col in range(dimx)]
-
- def identity(self, dim):
- # check if valid dimension
- if dim < 1:
- raise ValueError, "Invalid size of matrix"
- else:
- self.dimx = dim
- self.dimy = dim
- self.value = [[0 for row in range(dim)] for col in range(dim)]
- for i in range(dim):
- self.value[i][i] = 1
-
- def show(self):
- for i in range(self.dimx):
- print self.value[i]
- print ' '
-
- def __add__(self, other):
- # check if correct dimensions
- if self.dimx != other.dimx or self.dimy != other.dimy:
- raise ValueError, "Matrices must be of equal dimensions to add"
- else:
- # add if correct dimensions
- res = matrix([[]])
- res.zero(self.dimx, self.dimy)
- for i in range(self.dimx):
- for j in range(self.dimy):
- res.value[i][j] = self.value[i][j] + other.value[i][j]
- return res
-
- def __sub__(self, other):
- # check if correct dimensions
- if self.dimx != other.dimx or self.dimy != other.dimy:
- raise ValueError, "Matrices must be of equal dimensions to subtract"
- else:
- # subtract if correct dimensions
- res = matrix([[]])
- res.zero(self.dimx, self.dimy)
- for i in range(self.dimx):
- for j in range(self.dimy):
- res.value[i][j] = self.value[i][j] - other.value[i][j]
- return res
-
- def __mul__(self, other):
- # check if correct dimensions
- if self.dimy != other.dimx:
- raise ValueError, "Matrices must be m*n and n*p to multiply"
- else:
- # subtract if correct dimensions
- res = matrix([[]])
- res.zero(self.dimx, other.dimy)
- for i in range(self.dimx):
- for j in range(other.dimy):
- for k in range(self.dimy):
- res.value[i][j] += self.value[i][k] * other.value[k][j]
- return res
-
- def transpose(self):
- # compute transpose
- res = matrix([[]])
- res.zero(self.dimy, self.dimx)
- for i in range(self.dimx):
- for j in range(self.dimy):
- res.value[j][i] = self.value[i][j]
- return res
-
- # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions
-
- def Cholesky(self, ztol=1.0e-5):
- # Computes the upper triangular Cholesky factorization of
- # a positive definite matrix.
- res = matrix([[]])
- res.zero(self.dimx, self.dimx)
-
- for i in range(self.dimx):
- S = sum([(res.value[k][i])**2 for k in range(i)])
- d = self.value[i][i] - S
- if abs(d) < ztol:
- res.value[i][i] = 0.0
- else:
- if d < 0.0:
- raise ValueError, "Matrix not positive-definite"
- res.value[i][i] = sqrt(d)
- for j in range(i+1, self.dimx):
- S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])
- if abs(S) < ztol:
- S = 0.0
- res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
- return res
-
- def CholeskyInverse(self):
- # Computes inverse of matrix given its Cholesky upper Triangular
- # decomposition of matrix.
- res = matrix([[]])
- res.zero(self.dimx, self.dimx)
-
- # Backward step for inverse.
- for j in reversed(range(self.dimx)):
- tjj = self.value[j][j]
- S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
- res.value[j][j] = 1.0/tjj**2 - S/tjj
- for i in reversed(range(j)):
- res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]
- return res
-
- def inverse(self):
- aux = self.Cholesky()
- res = aux.CholeskyInverse()
- return res
-
- def __repr__(self):
- return repr(self.value)
-
-
-########################################
-
-def filter(x, P):
- for n in range(len(measurements)):
-
- # prediction
- x = (F * x) + B*u
- P = F * P * F.transpose()
- print 'predicted:'
- x.show()
-
- # measurement update
- Z = matrix([measurements[n]])
- y = Z.transpose() - (H * x)
- S = H * P * H.transpose() + R
- K = P * H.transpose() * S.inverse()
- x = x + (K * y)
- P = (I - (K * H)) * P
- print 'measure :'
- x.show()
-
- print 'x= '
- x.show()
- print 'P= '
- P.show()
-
-########################################
-
-print "### 6-dimensional example ###"
-
-measurements = [[5., 5. ,5.], [5., 6. ,5.], [5., 7. ,5.], [5., 8. ,5.], [5., 9. ,5.], [5., 10. ,5.]]
-initial_vxyz = [5., 4., 5.]
-
-
-dt = 1.
-
-
-# x : [x, y, z, vx, vy, vz]
-x = matrix([
- [0.] ,
- [0.] ,
- [0.] ,
- [initial_vxyz[0]],
- [initial_vxyz[1]],
- [initial_vxyz[2]]
- ]) # initial state (location and velocity)
-
-
-F = matrix([
- [1., 0., 0., dt, 0. ,0.],
- [0., 1., 0., 0., dt ,0.],
- [0., 0., 1., 0. ,0. ,dt],
- [0., 0., 0., 1. ,0., 0.],
- [0., 0., 0., 0. ,1., 0.],
- [0., 0., 0., 0. ,0., 1.],
- ])# next state function
-
-u = matrix([
- [0.],
- [1.],
- [0.]
- ]) # external motion.. acceleration here. ax, ay, az
-
-B = matrix([
- [0.5*(dt**2), 0. , 0. ],
- [0. , 0.5*(dt**2), 0. ],
- [0. , 0. , 0.5*(dt**2)],
- [dt , 0. , 0. ],
- [0. , dt , 0. ],
- [0. , 0. , dt ]
- ]) # control input function
-
-P = matrix([
- [1.5 , 0. , 0. , 0. ,0. ,0. ],
- [0. , 1.5 , 0. , 0. ,0. ,0. ],
- [0. , 0. , 1.5 , 0. ,0. ,0. ],
- [0. , 0. , 0. , 0.2 ,0. ,0. ],
- [0. , 0. , 0. , 0. ,0.2 ,0. ],
- [0. , 0. , 0. , 0. ,0. ,0.2]
- ])# initial uncertainty
-
-H = matrix([
- [0. , 0. , 0. , 1. ,0. ,0.],
- [0. , 0. , 0. , 0. ,1. ,0.],
- [0. , 0. , 0. , 0. ,0. ,1.]
- ]) # mapping function
-
-R = matrix([[.1 , 0., 0.],
- [ 0.,.1 , 0.],
- [ 0., 0., .1],
- ])# measurement uncertainty
-
-I = matrix([
- [1. , 0. , 0. , 0 ,0. ,0.],
- [0. , 1. , 0. , 0 ,0. ,0.],
- [0. , 0. , 1. , 0 ,0. ,0.],
- [0. , 0. , 0. , 1. ,0. ,0.],
- [0. , 0. , 0. , 0. ,1. ,0.],
- [0. , 0. , 0. , 0. ,0. ,1.]
- ]) # Identity matrix
-
-filter(x, P)
diff --git a/mission_planner_stack/pose_server_python/src/kalman_estimator.py b/mission_planner_stack/pose_server_python/src/kalman_estimator.py
index 9d3d5e8..f15d4b9 100644
--- a/mission_planner_stack/pose_server_python/src/kalman_estimator.py
+++ b/mission_planner_stack/pose_server_python/src/kalman_estimator.py
@@ -152,51 +152,19 @@ class matrix:
return self.value[indx-1][indy-1]
-def kalman_estimate(x, P, measurement, u):
+def kalman_estimate(x, P, measurement):
dt = 0.1
- F = matrix([
- [1., 0., 0., dt, 0. ,0.],
- [0., 1., 0., 0., dt ,0.],
- [0., 0., 1., 0. ,0. ,dt],
- [0., 0., 0., 1. ,0., 0.],
- [0., 0., 0., 0. ,1., 0.],
- [0., 0., 0., 0. ,0., 1.],
- ])# next state function
-
- B = matrix([
- [0.5*(dt**2), 0. , 0. ],
- [0. , 0.5*(dt**2), 0. ],
- [0. , 0. , 0.5*(dt**2)],
- [dt , 0. , 0. ],
- [0. , dt , 0. ],
- [0. , 0. , dt ]
- ]) # control input function
-
- H = matrix([
- [0. , 0. , 0. , 1. ,0. ,0.],
- [0. , 0. , 0. , 0. ,1. ,0.],
- [0. , 0. , 0. , 0. ,0. ,1.]
- ]) # mapping function
-
- R = matrix([[.1 , 0., 0.],
- [ 0.,.1 , 0.],
- [ 0., 0., .1],
- ])# measurement uncertainty
-
- I = matrix([
- [1. , 0. , 0. , 0 ,0. ,0.],
- [0. , 1. , 0. , 0 ,0. ,0.],
- [0. , 0. , 1. , 0 ,0. ,0.],
- [0. , 0. , 0. , 1. ,0. ,0.],
- [0. , 0. , 0. , 0. ,1. ,0.],
- [0. , 0. , 0. , 0. ,0. ,1.]
- ]) # Identity matrix
+ F = matrix([[1., 0., dt, 0.], [0., 1., 0., dt], [0., 0., 1., 0.], [0., 0., 0., 1.]])# next state function
+ H = matrix([[0., 0., 1., 0.], [0., 0, 0., 1.]])# measurement function
+ R = matrix([[.1, 0.], [0., .1]])# measurement uncertainty
+ I = matrix([[1., 0., 0., 0.,], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) # identity matrix
+ u = matrix([[0.], [0.], [0.], [0.]]) # external motion
# prediction
# x = (F * x) + u
- x = (F * x) + (B * u)
+ x = (F * x)
P = F * P * F.transpose()
# measurement update
diff --git a/mission_planner_stack/pose_server_python/src/main.py b/mission_planner_stack/pose_server_python/src/main.py
old mode 100755
new mode 100644
index bb81665..07da73f
--- a/mission_planner_stack/pose_server_python/src/main.py
+++ b/mission_planner_stack/pose_server_python/src/main.py
@@ -1,4 +1,4 @@
-import roslib;roslib.load_manifest('mission_planner');
+import roslib;roslib.load_manifest('pose_server_python')
import rospy
import rospy
import time
@@ -8,48 +8,28 @@ import kraken_msgs
import kraken_msgs
from kraken_msgs.msg._absoluteRPY import absoluteRPY
from kraken_msgs.msg._dvlData import dvlData
-from kraken_msgs.msg._imuData import imuData
from kraken_msgs.msg._positionData import positionData
from kraken_msgs.msg._stateData import stateData
from kalman_estimator import *
-import time
-
-DEBUG = False
-
dt = 0.1
-NUM_VARIABLE_IN_STATE = 6
-INDEX_VEL_X = 4
-INDEX_VEL_Y = 5
-INDEX_VEL_Z = 6
+NUM_VARIABLE_IN_STATE = 4
+INDEX_VEL_X = 3
+INDEX_VEL_Y = 4
CONVERTED_TO_WORLD = False
-ACC_MATRIX_POPULATED = False
FIRST_ITERATION = True
-FIRST_ITERATION_VEL = True
base_roll = 0
base_pitch = 0
base_yaw = 0
-prev_vel = [0., 0., 0.]
-prev_time = time.time()
# state = [position-x, position-y, velocity-x, velocity-y]
-state = matrix([[0.], [0.], [0.], [0.], [0.], [0.]]) # initial state (location and velocity)
-statefilled = 3
-measurements = [[0.0, 0.0, 0.]]
-P = matrix([
- [1.5 , 0. , 0. , 0. ,0. ,0. ],
- [0. , 1.5 , 0. , 0. ,0. ,0. ],
- [0. , 0. , 1.5 , 0. ,0. ,0. ],
- [0. , 0. , 0. , 0.2 ,0. ,0. ],
- [0. , 0. , 0. , 0. ,0.2 ,0. ],
- [0. , 0. , 0. , 0. ,0. ,0.2]
- ])# initial uncertainty
-
-u = matrix([[0.], [0.], [0.]])
+state = matrix([[0.0], [0.0], [0.], [0.]]) # initial state (location and velocity)
+statefilled = 2
+measurements = [[0.0, 0.0]]
+P = matrix([[1000., 0., 0., 0.], [0., 1000., 0., 0.], [0., 0., 0, 0.], [0., 0., 0., 0]])# initial uncertainty
oldtime = 0
-
def dvlCallback2(dvl):
global oldtime
t = dvl.data
@@ -74,13 +54,8 @@ def transformCallback(abrpy):
global base_pitch
global base_yaw
- if DEBUG:
-
- print 'entered absolute rpy'
-
- vx = measurements[-1][0]
- vy = measurements[-1][1]
- vz = measurements[-1][2]
+ vx = state.getvalue(INDEX_VEL_X, 1)
+ vy = state.getvalue(INDEX_VEL_Y, 1)
# yaw, pitch, roll
@@ -95,19 +70,19 @@ def transformCallback(abrpy):
base_yaw = yaw
FIRST_ITERATION = False
- # print "IMU (Un-Corrected): ",
- # print round(roll, 2),
- # print round(pitch, 2),
- # print round(yaw, 2)
+ print "IMU (Un-Corrected): ",
+ print round(roll, 2),
+ print round(pitch, 2),
+ print round(yaw, 2)
roll = roll - base_roll
pitch = pitch - base_pitch
yaw = yaw - base_yaw
- # print "IMU (Corrected): ",
- # print round(roll, 2),
- # print round(pitch, 2),
- # print round(yaw, 2)
+ print "IMU (Corrected): ",
+ print round(roll, 2),
+ print round(pitch, 2),
+ print round(yaw, 2)
## Convert the roll, pitch and yaw to radians.
@@ -147,17 +122,11 @@ def transformCallback(abrpy):
# bodytoworld.show()
- vel_wrt_body = matrix([[vx],[vy],[vz]])
+ vel_wrt_body = matrix([[vx],[vy],[0.]])
vel_wrt_world = bodytoworld * vel_wrt_body
- state.setvalue(INDEX_VEL_X, 1, vel_wrt_world.getvalue(1, 1))
- state.setvalue(INDEX_VEL_Y, 1, vel_wrt_world.getvalue(2, 1))
- state.setvalue(INDEX_VEL_Z, 1, vel_wrt_world.getvalue(3, 1))
-
- statefilled += 3
-
- # vel_wrt_world.show()
+ vel_wrt_world.show()
CONVERTED_TO_WORLD = True
@@ -167,22 +136,10 @@ def dvlCallback(dvl):
global state
global measurements
global statefilled
- global FIRST_ITERATION_VEL
- global u
- global prev_time
- global ACC_MATRIX_POPULATED
# print dvl.data
- roll = dvl.data[0]
- pitch = dvl.data[1]
- yaw = dvl.data[2]
-
- print "DVL: ", roll, pitch, yaw
-
- if DEBUG:
-
- print "Entered DVL callback!"
+ # print "Entered DVL callback!"
## Extract from the variable dvl
@@ -191,83 +148,33 @@ def dvlCallback(dvl):
vx = dvl.data[3]
vy = -1 * dvl.data[4]
- vz = -1 * dvl.data[5]
-
- # print "DVL: ", round(vx, 2), round(vy, 2), round(vz, 2)
-
- if DEBUG:
-
- print "DVL: vz: ", round(vz, 2)
-
- # print vx, vy, vz
-
- if FIRST_ITERATION_VEL:
-
- global prev_vel
- prev_vel = [vx, vy, vz]
- FIRST_ITERATION_VEL = False
- dt = time.time() - prev_time
- prev_time = time.time()
+ # print vx, vy
- ax = (vx - prev_vel[0]) / dt
- ay = (vy - prev_vel[1]) / dt
- az = (vz - prev_vel[2]) / dt
-
- u = matrix([[ax], [ay], [az]])
+ roll = dvl.data[0]
+ pitch = dvl.data[1]
+ yaw = dvl.data[2]
# print "DVL: ", roll, pitch, yaw
## End extract step
- this_iteration_measurement = [vx, vy, vz]
+ this_iteration_measurement = [vx, vy]
- ACC_MATRIX_POPULATED = True
+ state.setvalue(INDEX_VEL_X, 1, vx)
+ state.setvalue(INDEX_VEL_Y, 1, vy)
- # print "States: ", statefilled
+ statefilled += 2
measurements.append(this_iteration_measurement)
-def imuCallback(imu):
-
- '''
- generates the u matrix that is given as an input to the Kalman estimator
-
- imu -> imuData, kraken_msgs
- '''
-
- return
-
- if DEBUG:
-
- print 'entered imu callback'
-
- global u
- global ACC_MATRIX_POPULATED
-
- ax = imu.data[3]
- ay = imu.data[4]
- az = imu.data[5]
-
- u = matrix([[ax], [ay], [az]])
-
- ACC_MATRIX_POPULATED = True
-
def publishStateAndPosition(state_matrix):
global position_publisher
global state_publisher
- if DEBUG:
-
- print "entered publish state-pos"
-
vx = state_matrix.getvalue(INDEX_VEL_X, 1)
vy = state_matrix.getvalue(INDEX_VEL_Y, 1)
- vz = state_matrix.getvalue(INDEX_VEL_Z, 1)
pos_x = state_matrix.getvalue(1, 1)
pos_y = state_matrix.getvalue(2, 1)
- pos_z = state_matrix.getvalue(3, 1)
-
- # print round(pos_x, 2), round(pos_y, 2), round(pos_z, 2), round(vx, 2), round(vy, 2), round(vz, 2)
present_position = positionData()
present_state = stateData()
@@ -294,8 +201,6 @@ def publishStateAndPosition(state_matrix):
absolute_rpy_topic_name = topicHeader.ABSOLUTE_RPY
dvl_topic_name = topicHeader.SENSOR_DVL
-imu_topic_name = topicHeader.SENSOR_IMU
-# print imu_topic_name
publish_state_topic_name = topicHeader.POSE_SERVER_STATE
publish_position_topic_name = topicHeader.PRESENT_POSE
@@ -308,7 +213,6 @@ rospy.init_node('pose_server_python_node', anonymous=True)
rospy.Subscriber(name=absolute_rpy_topic_name, data_class=absoluteRPY, callback=transformCallback)
rospy.Subscriber(name=dvl_topic_name, data_class=dvlData, callback=dvlCallback)
-rospy.Subscriber(name=imu_topic_name, data_class=imuData, callback=imuCallback)
position_publisher = rospy.Publisher(publish_position_topic_name, positionData, queue_size=10)
state_publisher = rospy.Publisher(publish_state_topic_name, stateData, queue_size=10)
@@ -317,23 +221,17 @@ state_publisher = rospy.Publisher(publish_state_topic_name, stateData, queue_siz
while(1):
- looprate = rospy.Rate(10)
-
# if all the data has been accumulated in the state variable
- if(statefilled >= NUM_VARIABLE_IN_STATE and CONVERTED_TO_WORLD and ACC_MATRIX_POPULATED):
+ if(statefilled >= NUM_VARIABLE_IN_STATE and CONVERTED_TO_WORLD):
- # if True:
-
- (new_state, new_P) = kalman_estimate(state, P, measurements[-1], u)
+ (new_state, new_P) = kalman_estimate(state, P, measurements[-1])
state.setvalue(1, 1, new_state.getvalue(1, 1))
state.setvalue(2, 1, new_state.getvalue(2, 1))
- state.setvalue(3, 1, new_state.getvalue(3, 1))
- statefilled = 3
+ statefilled = 2
CONVERTED_TO_WORLD = False
- ACC_MATRIX_POPULATED = False
publishStateAndPosition(state)
@@ -344,6 +242,4 @@ while(1):
P = matrix(new_P.value)
- looprate.sleep()
-
rospy.spin()
diff --git a/sensor_stack/nqDVL/src/nqDVL.py b/sensor_stack/nqDVL/src/nqDVL.py
index e9fc9cc..54ca82c 100755
--- a/sensor_stack/nqDVL/src/nqDVL.py
+++ b/sensor_stack/nqDVL/src/nqDVL.py
@@ -98,37 +98,33 @@ def getVal(rawdata):
global pubData
floatData = []
data = rawdata.split('\r')
- try:
- for i in range(1,len(data)-1):
- datal = (data[i]).split(',')
-
- for j in range(1,len(datal)):
- if (datal[j] == 'A'):
- floatData += [1.0]
- elif(datal[j] == 'I'):
- floatData += [0.0]
- else:
- floatData += [float(datal[j])]
-
- print floatData,len(floatData)
-
- if(len(floatData) == 24):
-
- #print (floatData)
-
- pubData.data[0] = floatData[1] #roll
- pubData.data[1] = floatData[0] #pitch
- pubData.data[2] = floatData[2] #yaw
- pubData.data[3] = -floatData[15]/100.0 #Longitudinal Vel
- pubData.data[4] = floatData[14]/100.0 #Y Vel
- pubData.data[5] = floatData[16]/100.0 #Normal Vel
- pubData.data[6] = floatData[22] #altitude
- pubData.data[7] = floatData[5] #depth (does not work in our model)
- pubData.data[8] = floatData[4] #temperature
- pubData.data[9] = floatData[7]
- except Exception, e:
- pass
- #Sound Velocity
+ for i in range(1,len(data)-1):
+ datal = (data[i]).split(',')
+
+ for j in range(1,len(datal)):
+ if (datal[j] == 'A'):
+ floatData += [1.0]
+ elif(datal[j] == 'I'):
+ floatData += [0.0]
+ else:
+ floatData += [float(datal[j])]
+
+ print floatData,len(floatData)
+
+ if(len(floatData) == 24):
+
+ #print (floatData)
+
+ pubData.data[0] = floatData[1] #roll
+ pubData.data[1] = floatData[0] #pitch
+ pubData.data[2] = floatData[2] #yaw
+ pubData.data[3] = -floatData[15]/100.0 #Longitudinal Vel
+ pubData.data[4] = floatData[14]/100.0 #Y Vel
+ pubData.data[5] = floatData[16]/100.0 #Normal Vel
+ pubData.data[6] = floatData[22] #altitude
+ pubData.data[7] = floatData[5] #depth (does not work in our model)
+ pubData.data[8] = floatData[4] #temperature
+ pubData.data[9] = floatData[7 ] #Sound Velocity
if __name__ == '__main__':
@@ -150,7 +146,7 @@ if __name__ == '__main__':
# print k
# print 'out'
- r = rospy.Rate(10)
+ r = rospy.Rate(5)
count = 1
# k = ''
while not rospy.is_shutdown():
diff --git a/sensor_stack/seabotix/cfg/cpp/seabotix/k_changerConfig.h b/sensor_stack/seabotix/cfg/cpp/seabotix/k_changerConfig.h
index 94fe7d5..4fa6090 100644
--- a/sensor_stack/seabotix/cfg/cpp/seabotix/k_changerConfig.h
+++ b/sensor_stack/seabotix/cfg/cpp/seabotix/k_changerConfig.h
@@ -7,6 +7,43 @@
//
// ********************************************************/
+/***********************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ ***********************************************************/
+
+// Author: Blaise Gassend
+
+
#ifndef __seabotix__K_CHANGERCONFIG_H__
#define __seabotix__K_CHANGERCONFIG_H__
@@ -345,7 +382,7 @@ public:
double Kdt;
//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
double Kit;
-//#line 218 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template"
+//#line 255 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template"
bool __fromMessage__(dynamic_reconfigure::Config &msg)
{
@@ -643,7 +680,7 @@ class k_changerConfigStatics
Default.convertParams();
//#line 233 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py"
__group_descriptions__.push_back(k_changerConfig::AbstractGroupDescriptionConstPtr(new k_changerConfig::GroupDescription<k_changerConfig::DEFAULT, k_changerConfig>(Default)));
-//#line 353 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template"
+//#line 390 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template"
for (std::vector<k_changerConfig::AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); ++i)
{
diff --git a/sensor_stack/seabotix/manifest.xml b/sensor_stack/seabotix/manifest.xml
index 71d80d5..e26d016 100644
--- a/sensor_stack/seabotix/manifest.xml
+++ b/sensor_stack/seabotix/manifest.xml
@@ -12,7 +12,6 @@
<depend package="roslib"/>
<depend package="std_msgs"/>
<depend package="kraken_msgs"/>
- <depend package="actionmsg"/>
<depend package="resources"/>
<depend package="dynamic_reconfigure"/>
</package>
diff --git a/sensor_stack/seabotix/src/control.py b/sensor_stack/seabotix/src/control.py
old mode 100755
new mode 100644
index 5775ee0..ed10555
--- a/sensor_stack/seabotix/src/control.py
+++ b/sensor_stack/seabotix/src/control.py
@@ -10,36 +10,16 @@ from kraken_msgs.msg import thrusterData4Thruster
from kraken_msgs.msg import imuData
from resources import topicHeader
-import sys
-
-# Yaw must be passed as the first argument to the command
-
-if(len(sys.argv) < 2):
-
- print "Enter yaw, to run this script."
- exit()
-
yaw = 0.0
-goal = float(sys.argv[1])
-
-base_yaw = 0.0
-FIRST_ITERATION = True
-
-Kp_left = -2.5
-Kd_left = -5.25
-Ki_left = -2.25
+goal = 150.0
-Kp_right = 2.5
-Kd_right = 5.25
-Ki_right = 2.25
+Kp_left = 1.27;
+Kd_left = 0.016;
+Ki_left = 0.00;
-# Kp_left = 1.27;
-# Kd_left = 0.046;
-# Ki_left = 0.00;
-
-# Kp_right = -1.27;
-# Kd_right = -0.016;
-# Ki_right = -0.00;
+Kp_right = -1.27;
+Kd_right = -0.016;
+Ki_right = -0.00;
errorI = 0.0
errorP = 0.0
@@ -52,18 +32,11 @@ def imuCB(dataIn):
global errorP
global errorD
global prevError
- global FIRST_ITERATION
- global base_yaw
yaw = dataIn.data[2]
- if FIRST_ITERATION:
-
- base_yaw = yaw
- FIRST_ITERATION = False
-
prevError = errorP
- errorP = base_yaw + goal - yaw
+ errorP = goal - yaw
print errorP
errorI = errorP + prevError
errorD = errorP - prevError
@@ -71,6 +44,7 @@ def imuCB(dataIn):
#thruster6Data.data = [0.0,0.0,0.0,0.0,0.0,0.0]
#thruster4Data.data = [0.0, 0.0, 0.0, 0.0]
+
if __name__ == '__main__':
thruster4Data=thrusterData4Thruster();
thruster6Data=thrusterData6Thruster();
@@ -88,9 +62,9 @@ if __name__ == '__main__':
thruster6Data.data[1] = 0.0
thruster6Data.data[2] = 0.0
thruster6Data.data[3] = 0.0
+
thruster6Data.data[4] = Kp_left*errorP + Kd_left*errorD + Ki_left*errorI
- thruster6Data.data[5] = -1 * thruster6Data.data[4]
- # thruster6Data.data[5] = Kp_right*errorP + Kd_right*errorD + Ki_right*errorI
+ thruster6Data.data[5] = Kp_right*errorP + Kd_right*errorD + Ki_right*errorI
thruster4Data.data[0] = thruster6Data.data[0]
thruster4Data.data[1] = thruster6Data.data[1]
diff --git a/sensor_stack/seabotix/src/control_camera.py b/sensor_stack/seabotix/src/control_camera.py
deleted file mode 100755
index a39df82..0000000
--- a/sensor_stack/seabotix/src/control_camera.py
+++ /dev/null
@@ -1,100 +0,0 @@
-#!/usr/bin/env python
-
-PKG = 'seabotix'
-
-import roslib; roslib.load_manifest(PKG)
-import serial
-import rospy
-from kraken_msgs.msg import thrusterData6Thruster
-from kraken_msgs.msg import thrusterData4Thruster
-from kraken_msgs.msg import imuData
-from resources import topicHeader
-
-import sys
-
-# Yaw must be passed as the first argument to the command
-
-if(len(sys.argv) < 2):
-
- print "Enter yaw, to run this script."
- exit()
-
-yaw = 0.0
-goal = float(sys.argv[1])
-
-base_yaw = 0.0
-FIRST_ITERATION = True
-
-Kp_left = 0.127;
-Kd_left = 0.0016;
-Ki_left = 0.000;
-
-Kp_right = -0.127;
-Kd_right = -0.0016;
-Ki_right = -0.000;
-
-errorI = 0.0
-errorP = 0.0
-errorD = 0.0
-prevError = 0.0
-
-def cameraCB(dataIn):
- global yaw
- global errorI
- global errorP
- global errorD
- global prevError
- global FIRST_ITERATION
- global base_yaw
-
- yaw = dataIn.data[2]
-
- if FIRST_ITERATION:
-
- base_yaw = yaw
- FIRST_ITERATION = False
-
- prevError = errorP
- errorP = base_yaw + goal - yaw
- print errorP
- errorI = errorP + prevError
- errorD = errorP - prevError
-
-#thruster6Data.data = [0.0,0.0,0.0,0.0,0.0,0.0]
-#thruster4Data.data = [0.0, 0.0, 0.0, 0.0]
-
-if __name__ == '__main__':
- thruster4Data=thrusterData4Thruster();
- thruster6Data=thrusterData6Thruster();
-
- rospy.init_node('Control', anonymous=True)
- sub = rospy.Subscriber('/kraken/buoy/coordinates/', 5, cameraCB)
- pub4 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER4, thrusterData4Thruster, queue_size = 2)
- pub6 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, queue_size = 2)
-
- r = rospy.Rate(10)
-
- while not rospy.is_shutdown():
-
- thruster6Data.data[0] = 0.0
- thruster6Data.data[1] = 0.0
- thruster6Data.data[2] = 0.0
- thruster6Data.data[3] = 0.0
-
- thrust_input = Kp_left*errorP + Kd_left*errorD + Ki_left*errorI
- thruster6Data.data[4] = int(round(thrust_input, 0))
- thruster6Data.data[5] = -1 * thruster6Data.data[4]
- # thruster6Data.data[5] = Kp_right*errorP + Kd_right*errorD + Ki_right*errorI
-
- thruster4Data.data[0] = thruster6Data.data[0]
- thruster4Data.data[1] = thruster6Data.data[1]
- thruster4Data.data[2] = thruster6Data.data[4]
- thruster4Data.data[3] = thruster6Data.data[5]
-
- #pub4.publish(thruster4Data)
- pub6.publish(thruster6Data)
-
-
- r.sleep()
-
-
diff --git a/sensor_stack/seabotix/src/depthCtrl.py b/sensor_stack/seabotix/src/depthCtrl.py
old mode 100755
new mode 100644
index dc8a0ef..6d37542
--- a/sensor_stack/seabotix/src/depthCtrl.py
+++ b/sensor_stack/seabotix/src/depthCtrl.py
@@ -11,21 +11,15 @@ from kraken_msgs.msg import dvlData
from resources import topicHeader
from dynamic_reconfigure.server import Server
from seabotix.cfg import k_changerConfig
-from actionmsg.msg import buoyActionFeedback
+offset = 0.8
+depth = 0.0
+depthGoal = 0.4
-offset = 0
-depth = 1.1
-depthGoal = 0.1
-PIXEL_RATIO=10000.
+Kp = 69
+Kd = 27
+Ki = 25
-# Kp = 100
-# Kd = 44
-# Ki = 0.5
-
-Kp = 500
-Kd = 100
-Ki = 0
# Use in case of mismatch
Kp_front = 200;
Kd_front = 0.5;
@@ -54,27 +48,16 @@ def dvlCB(dataIn):
offset = dataIn.data[6]
i = i+1
print i,offset
- if dataIn.data[6]>2:
- return;
+
depth = offset - dataIn.data[6]
prevError = errorP
errorP = depthGoal - depth
-
-
- print 'ErrorP: ',errorP
-
+ print 'Error',errorP
errorI = errorP + prevError
errorD = errorP - prevError
-def goalCB(goalin):
- global depthGoal
- global PIXEL_RATIO
-
- # depthGoal=depthGoal+ goalin.feedback.errory*1.0/PIXEL_RATIO
- # print 'new goal depth=',depthGoal,' goalin=',goalin.feedback.errory,' val=',goalin.feedback.errory*1.0/PIXEL_RATIO;
-
thruster6Data = thrusterData6Thruster()
thruster4Data = thrusterData4Thruster()
@@ -91,27 +74,23 @@ if __name__ == '__main__':
rospy.init_node('Control', anonymous=True)
sub = rospy.Subscriber(topicHeader.SENSOR_DVL, dvlData, dvlCB)
-
- goalsub=rospy.Subscriber('/buoy/feedback',buoyActionFeedback,goalCB)
-
pub4 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER4, thrusterData4Thruster, queue_size = 2)
pub6 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, queue_size = 2)
- # srv=Server(k_changerConfig,callback)
+ srv=Server(k_changerConfig,callback)
r = rospy.Rate(10)
- thruster6Data.data[2] = 0.0
- thruster6Data.data[3] = 0.0
- thruster6Data.data[4] = 0.0
- thruster6Data.data[5] = 0.0
while not rospy.is_shutdown():
- # front thruster
- thruster6Data.data[0] = (Kp + 40)*errorP + (Kd)*errorD + (Ki + 0.1)*errorI
-
- # rear thruster
+ thruster6Data.data[0] = Kp*errorP + Kd*errorD + Ki*errorI
+ #print Kp,Kd,Ki,errorP,errorD,errorI
+ #print thruster6Data.data[0]
thruster6Data.data[1] = Kp*errorP + Kd*errorD + Ki*errorI
+ thruster6Data.data[2] = 0.0
+ thruster6Data.data[3] = 0.0
+ thruster6Data.data[4] = 0.0
+ thruster6Data.data[5] = 0.0
thruster4Data.data[0] = thruster6Data.data[0]
thruster4Data.data[1] = thruster6Data.data[1]
diff --git a/sensor_stack/seabotix/src/main-control.py b/sensor_stack/seabotix/src/main-control.py
deleted file mode 100644
index 6b7fe35..0000000
--- a/sensor_stack/seabotix/src/main-control.py
+++ /dev/null
@@ -1,163 +0,0 @@
-#!/usr/bin/env python
-
-PKG = 'seabotix'
-
-import roslib; roslib.load_manifest(PKG)
-import serial
-import rospy
-from kraken_msgs.msg import thrusterData6Thruster
-from kraken_msgs.msg import thrusterData4Thruster
-from kraken_msgs.msg import imuData
-from kraken_msgs.msg import positionData
-from resources import topicHeader
-
-import sys
-
-import time
-
-goal = 0.
-first_iteration = True
-threshold= 0.0
-Kp_left = 0.1
-Kd_left = 0.00
-Ki_left = 0.0000
-
-Kp_depth = 0.16
-Kd_depth = 0.1
-Ki_depth = 0
-
-errorI = 0.0
-errorP = 0.0
-errorD = 0.0
-prevError = 0.0
-
-errorI2 = 0.0
-errorP2 = 0.0
-errorD2 = 0.0
-prevError2 = 0.0
-
-stop_vehicle = False
-
-thrust_input = 20.
-
-def imuCallback(imu):
-
- global prev_acc
- global acc
- global first_iteration
-# global prev_time
- global stop_vehicle
- global threshold
-
- acc = imu.data[3]
-
- print acc
-
- if first_iteration:
-
- prev_acc = acc
- first_iteration = False
-
- pres_time = time.time()
-
- difftime = pres_time - prev_time
-
- prev_time = pres_time
-
- differential = (prev_acc - acc) #/ difftime
-
- if abs(differential) > threshold:
-
- #stop_vehicle = True
- pass
-
-
-prev_acc = 0.
-acc = 0.
-first_iteration = True
-stop_vehicle = False
-prev_time = time.time()
-threshold = 100.
-dt =0.
-
-def buoyCoordCB(buoy_coord):
- global errorI
- global errorP
- global errorD
- global prevError
- global first_iteration
- global goal
- global prev_time
- global errorI2
- global errorP2
- global errorD2
- global prevError2
- global dt
- if first_iteration:
- prev_time = time.time()
- first_iteration = False
- dt=1.0
- else:
- pres_time = time.time()
- dt = pres_time - prev_time
- prev_time = pres_time
- x = buoy_coord.x_coordinate
- y = buoy_coord.y_coordinate
-
- prevError = errorP
- errorP = x - goal
- print 'errorP: ', errorP
- errorI = (errorP + errorI)*dt
- errorD = (errorP - prevError)/dt
-
- prevError2 = errorP2
- errorP2 = goal - y
- print 'errorP2: ', errorP2
- errorI2 = (errorP2 + errorI2)*dt
- errorD2 = (errorP2 - prevError2)/dt
-
-if __name__ == '__main__':
-
- thruster4Data=thrusterData4Thruster();
- thruster6Data=thrusterData6Thruster();
-
- rospy.init_node('buoy_control_node', anonymous=True)
- sub1 = rospy.Subscriber('/kraken/buoy/coordinates', positionData, buoyCoordCB)
- #sub = rospy.Subscriber(topicHeader.SENSOR_IMU, imuData, imuCallback)
- pub4 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER4, thrusterData4Thruster, queue_size = 1)
- pub6 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, queue_size = 1)
-
- r = rospy.Rate(10)
-
- while not rospy.is_shutdown():
-
-# x = int(raw_input("Enter x: "))
-# buoyCoord(x)
-
- thruster6Data.data[0] = 0.0
- thruster6Data.data[1] = 0.0
- thruster6Data.data[2] = 0.0
- thruster6Data.data[3] = 0.0
-
- thruster6Data.data[4] = (Kp_left*errorP + Kd_left*errorD + Ki_left*errorI) + thrust_input
- thruster6Data.data[5] = -1*(Kp_left*errorP + Kd_left*errorD + Ki_left*errorI) + thrust_input
-
- thruster6Data.data[0] = 0. # Kp_depth*errorP2 + Kd_depth*errorD2 + Ki_depth*errorI2
- thruster6Data.data[1] = 0. # thruster6Data.data[0]
-
- thruster4Data.data[0] = thruster6Data.data[0]
- thruster4Data.data[1] = thruster6Data.data[1]
- thruster4Data.data[2] = thruster6Data.data[4]
- thruster4Data.data[3] = thruster6Data.data[5]
-
- if stop_vehicle:
-
- thruster6Data.data[4] = 0.
- thruster6Data.data[5] = 0.
-
- thruster6Data.data[0] = 0.
- thruster6Data.data[1] = 0.
- # pub4.publish(thruster4Data)
- pub6.publish(thruster6Data)
-
- r.sleep()
diff --git a/sensor_stack/seabotix/src/rumf.py b/sensor_stack/seabotix/src/rumf.py
deleted file mode 100644
index 3d072a5..0000000
--- a/sensor_stack/seabotix/src/rumf.py
+++ /dev/null
@@ -1,104 +0,0 @@
-#!/usr/bin/env python
-
-PKG = 'seabotix'
-
-import roslib; roslib.load_manifest(PKG)
-import serial
-import rospy
-from kraken_msgs.msg import thrusterData6Thruster
-from kraken_msgs.msg import thrusterData4Thruster
-from kraken_msgs.msg import imuData
-from resources import topicHeader
-
-thruster4Data=thrusterData4Thruster();
-thruster6Data=thrusterData6Thruster();
-
-
-def heave(vel):
- global thruster6Data
- thruster6Data.data[0] = vel
- thruster6Data.data[1] = vel
- pub6.publish(thruster6Data)
-
-def stopHeave():
- global thruster6Data
- thruster6Data.data[0] = 0.0
- thruster6Data.data[1] = 0.0
- pub6.publish(thruster6Data)
-
-def surge(vel):
- global thruster6Data
- thruster6Data.data[4] = vel
- thruster6Data.data[5] = vel
- pub6.publish(thruster6Data)
-
-def stopSurge():
- global thruster6Data
- thruster6Data.data[4] = 0.0
- thruster6Data.data[5] = 0.0
- pub6.publish(thruster6Data)
-
-
-# def imuCB(dataIn):
-# global yaw
-# global errorI
-# global errorP
-# global errorD
-# global prevError
-
-# yaw = dataIn.data[2]
-
-# prevError = errorP
-# errorP = goal - yaw
-# print errorP
-# errorI = errorP + prevError
-# errorD = errorP - prevError
-
-#thruster6Data.data = [0.0,0.0,0.0,0.0,0.0,0.0]
-#thruster4Data.data = [0.0, 0.0, 0.0, 0.0]
- thruster6Data.data[0] = 0.0
- thruster6Data.data[1] = 0.0
- thruster6Data.data[2] = 0.0
- thruster6Data.data[3] = 0.0
- thruster6Data.data[4] = 0.0
- thruster6Data.data[5] = 0.0
-
-
-if __name__ == '__main__':
-
- rospy.init_node('Control', anonymous=True)
- # sub = rospy.Subscriber(topicHeader.SENSOR_IMU, imuData, imuCB)
- pub4 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER4, thrusterData4Thruster, queue_size = 2)
- pub6 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, queue_size = 2)
-
- r = rospy.Rate(10)
-
- # while not rospy.is_shutdown():
-
- # thruster6Data.data[0] = 0.0
- # thruster6Data.data[1] = 0.0
- # thruster6Data.data[2] = 0.0
- # thruster6Data.data[3] = 0.0
-
- # thruster6Data.data[4] = Kp_left*errorP + Kd_left*errorD + Ki_left*errorI
- # thruster6Data.data[5] = Kp_right*errorP + Kd_right*errorD + Ki_right*errorI
-
- # thruster4Data.data[0] = thruster6Data.data[0]
- # thruster4Data.data[1] = thruster6Data.data[1]
- # thruster4Data.data[2] = thruster6Data.data[4]
- # thruster4Data.data[3] = thruster6Data.data[5]
-
- #pub4.publish(thruster4Data)
- #pub6.publish(thruster6Data)
-
- heave(50)
- rospy.sleep(3)
- stopHeave()
- rospy.sleep(1)
- surge(5)
- rospy.sleep(3)
- stopHeave()
- stopSurge()
- # r.sleep()
-
-
diff --git a/sensor_stack/seabotix/src/seabotix.py b/sensor_stack/seabotix/src/seabotix.py
index 6873afc..e577261 100755
--- a/sensor_stack/seabotix/src/seabotix.py
+++ b/sensor_stack/seabotix/src/seabotix.py
@@ -15,8 +15,7 @@ from resources import topicHeader
dataString = ''
-import os
-import signal
+
sb = serial.Serial('/dev/ttyACM0', 9600)
@@ -24,28 +23,6 @@ sb = serial.Serial('/dev/ttyACM0', 9600)
sb.stopbits = 1
-def stopThrustersNow(signal, frame):
-
- print "Stopping thrusters now!"
- print os.getcwd()
- data = [[0x60,0x80,0x64], #depth Back
- [0x50,0x80,0x64], #Surge Left
- [0x5A,0x80,0x64], #depth Front
- [0x52,0x80,0x64],
- [0x5C,0x80,0x64], #Surge Right
- [0x5E,0x80,0x64]
- ]
- global sb
- print "Cycle Started"
- for i in range(0,6):
- for j in range(0,3):
- sb.write(chr(data[i][j]))
- print "Thruster",(i+1),"stopped"
-
- sb.close()
- exit()
-
-
def initSerial():
if (not sb.isOpen) :
sb.open()
@@ -57,13 +34,13 @@ def initSerial():
print 'Error in opening port'
#data to be sent to Arduino
-data = [[0x60,0,0x64], #depth Back
- [0x50,0,0x64], #Surge Left
- [0x5A,0,0x64], #depth Front
- [0x52,0,0x64],
- [0x5C,0,0x64], #Surge Right
- [0x5E,0,0x64]
- ]
+data = [[0x5A,0,0x64],
+ [0x60,0,0x64],
+ [0x50,0,0x64],
+ [0x5E,0,0x64],
+ [0x52,0,0x64],
+ [0x5C,0,0x64]]
+
def seabotixCB(dataI):
global data
@@ -86,11 +63,9 @@ def seabotixCB(dataI):
if __name__ == '__main__':
- signal.signal(signal.SIGINT, stopThrustersNow)
-
initSerial()
- rospy.init_node('seabotix_py_thrusters', anonymous=True)
+ rospy.init_node('Thruster', anonymous=True)
sub = rospy.Subscriber(topicHeader.CONTROL_SEABOTIX, seabotix, seabotixCB)
@@ -116,12 +91,14 @@ if __name__ == '__main__':
#print speed
+ print sb.readline()
while not rospy.is_shutdown():
- for i in range(0,6):
- for j in range(0,3):
- sb.write(chr(data[i][j]))
- print "Thruster ", (i+1)
- r.sleep()
+ for i in range(0,6):
+ for j in range(0,3):
+ sb.write(str(chr(int(data[i][j]))))
+ print sb.readline()
+
+ r.sleep()
sb.close()
diff --git a/sensor_stack/seabotix/src/seabotix/cfg/k_changerConfig.py b/sensor_stack/seabotix/src/seabotix/cfg/k_changerConfig.py
index 70b46f2..7df45df 100644
--- a/sensor_stack/seabotix/src/seabotix/cfg/k_changerConfig.py
+++ b/sensor_stack/seabotix/src/seabotix/cfg/k_changerConfig.py
@@ -6,6 +6,40 @@
##
## ********************************************************/
+##**********************************************************
+## Software License Agreement (BSD License)
+##
+## Copyright (c) 2008, Willow Garage, Inc.
+## All rights reserved.
+##
+## Redistribution and use in source and binary forms, with or without
+## modification, are permitted provided that the following conditions
+## are met:
+##
+## * Redistributions of source code must retain the above copyright
+## notice, this list of conditions and the following disclaimer.
+## * Redistributions in binary form must reproduce the above
+## copyright notice, this list of conditions and the following
+## disclaimer in the documentation and/or other materials provided
+## with the distribution.
+## * Neither the name of the Willow Garage nor the names of its
+## contributors may be used to endorse or promote products derived
+## from this software without specific prior written permission.
+##
+## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+## "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+## LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+## FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+## COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+## INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+## BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+## LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+## CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+## LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+## ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+## POSSIBILITY OF SUCH DAMAGE.
+##**********************************************************/
+
from dynamic_reconfigure.encoding import extract_params
inf = float('inf')
diff --git a/sensor_stack/seabotix/src/stopKraken.py b/sensor_stack/seabotix/src/stopKraken.py
old mode 100755
new mode 100644
index 7f8676b..c644de1
--- a/sensor_stack/seabotix/src/stopKraken.py
+++ b/sensor_stack/seabotix/src/stopKraken.py
@@ -2,11 +2,31 @@
PKG = 'seabotix'
+# stopKraken.py
+
+# This module uses the serial library in python.
+
+# It creates a handle for the serial port in the Arduino and then writes
+# the data to be fed to the thrusters directly to this serial port.
+
+# It does so using a while loop. (which may be redundant)
+
+# The value "0x7F" is the zero for the seabotix thrusters. This is
+# the reason that this value is part of the data that is being written
+# to the serial port.
+
+# TODO:
+
+# - Check how the thrusters work
+# If only publishing once is enough, then the while loop is redundant,
+# and we can stop the loop after publishing once.
+# - Write a bash script to start this up directly from the repo root.
+
+
from time import sleep
import roslib; roslib.load_manifest(PKG)
import serial
-
import rospy
from kraken_msgs.msg import seabotix
@@ -14,7 +34,7 @@ dataString = ''
sb = serial.Serial('/dev/ttyACM0', 9600)
-#serial config
+# serial config
sb.stopbits = 1
def initSerial():
@@ -27,61 +47,46 @@ def initSerial():
else:
print 'Error in opening port'
-
-def seabotixCB(data):
- global dataString
- dataString = ''
- checksum = '0x00'
-
- for i in data:
- dataString += chr(data.data[i])
- checksum += data.data[i]
- dataString += chr(checksum)
- sb.write(dataString)
-
-
-
if __name__ == '__main__':
initSerial()
- rospy.init_node('Thruster', anonymous=True)
- sub = rospy.Subscriber('/kraken/seabotix', seabotix, seabotixCB)
+ rospy.init_node('stopKraken', anonymous=True)
- #count = 0 # variable to check frequency
- #add = [0X60,0X52,0X5A,0X50,0X5C,0X5E]
- #speed = [0X62,0X62,0X62,0X62,0X62,0X62]
- #speedMax = [0X64,0X64,0X64,0X64,0X64,0X64]
+ # count = 0 # variable to check frequency
+ # add = [0X60,0X52,0X5A,0X50,0X5C,0X5E]
+ # speed = [0X62,0X62,0X62,0X62,0X62,0X62]
+ # speedMax = [0X64,0X64,0X64,0X64,0X64,0X64]
data = [[0x60,0x7F,0x64],
[0x52,0x7F,0x64],
[0x5A,0x7F,0x64],
[0x50,0x7F,0x64],
[0x5C,0x7F,0x64],
[0x5E,0x7F,0x64]]
- #add[0] = 50
- #add[1] = '56'
- #add[2] = '5A'
- #add[3] = '5E'
- #add[4] = '52'
- #add[5] = '58'
- #add[4] = '60'
- #add[5] = '5C'
- r = rospy.Rate(1)
+ # add[0] = 50
+ # add[1] = '56'
+ # add[2] = '5A'
+ # add[3] = '5E'
+ # add[4] = '52'
+ # add[5] = '58'
+ # add[4] = '60'
+ # add[5] = '5C'
- print 'running'
+ r = rospy.Rate(1)
- #print speed
+ print 'While Loop Started'
- print sb.readline()
while not rospy.is_shutdown():
- for i in range(0,6):
- for j in range(0,3):
- sb.write(str(chr(int(data[i][j]))))
- print sb.readline()
+ print 'Cycle Started'
+ for i in range(0,6):
+ for j in range(0,3):
+ sb.write(str(chr(int(data[i][j]))))
- sb.close()
- exit()
+ print 'Cycle Ended'
r.sleep()
sb.close()
+
+ print "Exited from the While Loop"
+
\ No newline at end of file
diff --git a/sensor_stack/seabotix/src/stopKraken1.py b/sensor_stack/seabotix/src/stopKraken1.py
deleted file mode 100644
index eca321f..0000000
--- a/sensor_stack/seabotix/src/stopKraken1.py
+++ /dev/null
@@ -1,91 +0,0 @@
-#!/usr/bin/env python
-
-PKG = 'seabotix'
-
-# stopKraken.py
-
-# This module uses the serial library in python.
-
-# It creates a handle for the serial port in the Arduino and then writes
-# the data to be fed to the thrusters directly to this serial port.
-
-# It does so using a while loop. (which may be redundant)
-
-# The value "0x7F" is the zero for the seabotix thrusters. This is
-# the reason that this value is part of the data that is being written
-# to the serial port.
-
-# TODO:
-
-# - Check how the thrusters work
-# If only publishing once is enough, then the while loop is redundant,
-# and we can stop the loop after publishing once.
-# - Write a bash script to start this up directly from the repo root.
-
-
-from time import sleep
-import roslib; roslib.load_manifest(PKG)
-import serial
-
-import rospy
-from kraken_msgs.msg import seabotix
-
-dataString = ''
-
-sb = serial.Serial('/dev/ttyACM0', 9600)
-
-# serial config
-sb.stopbits = 1
-
-def initSerial():
- if (sb.isOpen == 0):
- sb.open()
-
- if (sb.isOpen) :
- print 'Serial port opened successfully'
-
- else:
- print 'Error in opening port'
-
-if __name__ == '__main__':
-
- initSerial()
-
- rospy.init_node('Thruster', anonymous=True)
- sub = rospy.Subscriber('/kraken/seabotix', seabotix, seabotixCB)
-
- # count = 0 # variable to check frequency
- # add = [0X60,0X52,0X5A,0X50,0X5C,0X5E]
- # speed = [0X62,0X62,0X62,0X62,0X62,0X62]
- # speedMax = [0X64,0X64,0X64,0X64,0X64,0X64]
- data = [[0x60,0x7F,0x64],
- [0x52,0x7F,0x64],
- [0x5A,0x7F,0x64],
- [0x50,0x7F,0x64],
- [0x5C,0x7F,0x64],
- [0x5E,0x7F,0x64]]
-
- # add[0] = 50
- # add[1] = '56'
- # add[2] = '5A'
- # add[3] = '5E'
- # add[4] = '52'
- # add[5] = '58'
- # add[4] = '60'
- # add[5] = '5C'
-
- r = rospy.Rate(1)
-
- print 'While Loop Started'
-
- while not rospy.is_shutdown():
- print 'Cycle Started'
- for i in range(0,6):
- for j in range(0,3):
- sb.write(str(chr(int(data[i][j]))))
-
- print 'Cycle Ended'
- r.sleep()
-
- sb.close()
-
\ No newline at end of file
diff --git a/sensor_stack/seabotix/src/surge.py b/sensor_stack/seabotix/src/surge.py
deleted file mode 100644
index 07fe6c2..0000000
--- a/sensor_stack/seabotix/src/surge.py
+++ /dev/null
@@ -1,87 +0,0 @@
-#!/usr/bin/env python
-
-PKG = 'seabotix'
-
-import roslib; roslib.load_manifest(PKG)
-import serial
-import rospy
-from kraken_msgs.msg import thrusterData6Thruster
-from kraken_msgs.msg import thrusterData4Thruster
-from kraken_msgs.msg import imuData
-from resources import topicHeader
-
-import sys
-
-import time
-
-def imuCallback(imu):
-
- print "Entered the callback!"
-
- global prev_acc
- global acc
- global first_iteration
- global prev_time
- global stop_vehicle
- global threshold
-
- acc = imu.data[3]
-
- if first_iteration:
-
- prev_acc = acc
- first_iteration = False
-
- pres_time = time.time()
-
- difftime = pres_time - prev_time
-
- prev_time = pres_time
-
- differential = (prev_acc - acc) / difftime
-
- if abs(differential) > threshold:
-
- stop_vehicle = True
-
-prev_acc = 0.
-acc = 0.
-first_iteration = True
-stop_vehicle = False
-prev_time = time.time()
-threshold = 100.
-thrust_input = 40.
-
-rospy.init_node('demonstration_surge_control', anonymous=True)
-sub = rospy.Subscriber(topicHeader.SENSOR_IMU, imuData, imuCallback)
-pub4 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER4, thrusterData4Thruster, queue_size = 2)
-pub6 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, queue_size = 2)
-
-while not rospy.is_shutdown():
-
- thruster4Data = thrusterData4Thruster()
- thruster6Data = thrusterData6Thruster()
-
- if stop_vehicle:
-
- thrust_input = 0.
-
- thruster6Data.data[0] = 0.0
- thruster6Data.data[1] = 0.0
- thruster6Data.data[2] = 0.0
- thruster6Data.data[3] = 0.0
-
- thruster6Data.data[4] = thrust_input
- thruster6Data.data[5] = thrust_input
-
- thruster4Data.data[0] = thruster6Data.data[0]
- thruster4Data.data[1] = thruster6Data.data[1]
- thruster4Data.data[2] = thruster6Data.data[4]
- thruster4Data.data[3] = thruster6Data.data[5]
-
- # pub4.publish(thruster4Data)
- pub6.publish(thruster6Data)
-
- if stop_vehicle:
-
- exit()
diff --git a/sensor_stack/seabotix/src/surgeControl-PID.py b/sensor_stack/seabotix/src/surgeControl-PID.py
deleted file mode 100644
index 68cbe24..0000000
--- a/sensor_stack/seabotix/src/surgeControl-PID.py
+++ /dev/null
@@ -1,98 +0,0 @@
-#!/usr/bin/env python
-
-PKG = 'seabotix'
-
-import roslib; roslib.load_manifest(PKG)
-import serial
-import rospy
-from kraken_msgs.msg import thrusterData6Thruster
-from kraken_msgs.msg import thrusterData4Thruster
-from kraken_msgs.msg import positionData
-from resources import topicHeader
-
-import sys
-
-def distance_between(a, b):
-
- '''
- returns the distance between two points a and b
-
- a and b are positionData() instances.
-
- Check the kraken_msgs package, (msg/positionData.msg) file
- for message details.
- '''
-
- return float(((a.x_coordinate - b.x_coordinate) ** 2 + (a.y_coordinate - b.y_coordinate) ** 2) ** 0.5)
-
-# Distance to be moved must be passed as the
-# first argument to the command
-
-total_dist = float(sys.argv[1])
-
-FIRST_ITERATION = True
-
-Kp = 1.27;
-Kd = 0.016;
-Ki = 0.00;
-
-errorI = 0.0
-errorP = 0.0
-errorD = 0.0
-prevError = 0.0
-initial_pose = positionData()
-
-def positionCB(pose):
- global yaw
- global errorI
- global errorP
- global errorD
- global prevError
- global FIRST_ITERATION
- global base_yaw
- global initial_pose
-
- if FIRST_ITERATION:
-
- initial_pose.x_coordinate = pose.x_coordinate
- initial_pose.y_coordinate = pose.y_coordinate
- FIRST_ITERATION = False
-
- prevError = errorP
- errorP = total_dist - distance_between(pose, initial_pose)
- print errorP
- errorI = errorP + prevError
- errorD = errorP - prevError
-
-if __name__ == '__main__':
-
- thruster4Data = thrusterData4Thruster();
- thruster6Data = thrusterData6Thruster();
-
- rospy.init_node('surge_control_node', anonymous=True)
- sub = rospy.Subscriber(topicHeader.SENSOR_IMU, positionData, positionCB)
- pub4 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER4, thrusterData4Thruster, queue_size = 2)
- pub6 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, queue_size = 2)
-
- r = rospy.Rate(10)
-
- while not rospy.is_shutdown():
-
- thruster6Data.data[0] = 0.0
- thruster6Data.data[1] = 0.0
- thruster6Data.data[2] = 0.0
- thruster6Data.data[3] = 0.0
-
- thruster6Data.data[4] = Kp*errorP + Kd*errorD + Ki*errorI
- thruster6Data.data[5] = thruster6Data.data[4]
- # thruster6Data.data[5] = Kp_right*errorP + Kd_right*errorD + Ki_right*errorI
-
- thruster4Data.data[0] = thruster6Data.data[0]
- thruster4Data.data[1] = thruster6Data.data[1]
- thruster4Data.data[2] = thruster6Data.data[4]
- thruster4Data.data[3] = thruster6Data.data[5]
-
- # pub4.publish(thruster4Data)
- pub6.publish(thruster6Data)
-
- r.sleep()
diff --git a/sensor_stack/seabotix/src/surgeControl.py b/sensor_stack/seabotix/src/surgeControl.py
deleted file mode 100755
index f86cef1..0000000
--- a/sensor_stack/seabotix/src/surgeControl.py
+++ /dev/null
@@ -1,156 +0,0 @@
-#!/usr/bin/env python
-
-PKG = 'seabotix'
-
-import roslib; roslib.load_manifest(PKG)
-import serial
-import rospy
-from kraken_msgs.msg import thrusterData6Thruster
-from kraken_msgs.msg import thrusterData4Thruster
-from kraken_msgs.msg import imuData
-from kraken_msgs.msg import positionData
-from resources import topicHeader
-
-import sys
-
-position_topic_name = topicHeader.PRESENT_POSE
-initial_pose_x = 0.
-initial_pose_y = 0.
-total_dist = 0.
-thrust_input = 0.
-
-goalx = float(raw_input("Enter goalx: "))
-goaly = float(raw_input("Enter goaly: "))
-
-# goalx = 5
-# goaly = 4
-
-## Equations
-
-max_thrust_input = 100.0
-
-FIRST_ITERATION = True
-THRUST_SET = True
-
-# if(len(sys.argv) < 2):
-
-# print "Enter yaw, to run this script."
-# exit()
-
-def distance_between(a, b):
-
- '''
- Distance between two points a and b
- a = (x1, y1)
- b = (x2, y2)
-
- Both a and b are tuples of length 2.
- '''
-
- return float(((a[0] - b[0])**2 + (a[1] - b[1])**2)**0.5)
-
-def positionCallback(pose):
-
- global initial_pose_x
- global initial_pose_y
- global goalx
- global goaly
- global total_dist
- global max_thrust_input
- global thrust_input
- global a
- global FIRST_ITERATION
- global THRUST_SET
-
- pose_x = pose.x_coordinate
- pose_y = pose.y_coordinate
-
- if FIRST_ITERATION:
-
- initial_pose_x = pose_x
- initial_pose_y = pose_y
- total_dist = distance_between((initial_pose_x, initial_pose_y), (goalx, goaly))
- # Calculate the constant a
- a = max_thrust_input / (0.25*total_dist)**2
-
- print total_dist, a
-
- FIRST_ITERATION = False
-
- present_dist = distance_between((initial_pose_x, initial_pose_y), (pose_x, pose_y))
-
- if present_dist == 0.:
-
- thrust_input = 100.0
-
- elif (present_dist < total_dist / 8.0):
-
- thrust_input = a * (present_dist)**2
-
- print "Parabola 1"
-
- elif (present_dist < total_dist / 4.0):
-
- thrust_input = max_thrust_input - a * (total_dist/4. - present_dist)**2
-
- print "Parabola 2"
-
- elif (present_dist < total_dist * 0.75):
-
- thrust_input = max_thrust_input
-
- print "Constant 3"
-
- elif (present_dist < total_dist * (0.75 + 1/8.0)):
-
- thrust_input = max_thrust_input - a * (present_dist - total_dist*0.75)**2
-
- print "Parabola 3"
-
- else:
-
- thrust_input = a * (total_dist - present_dist)**2
-
- print "Parabola 4"
-
- thrust_input = thrust_input * 100
- print present_dist, thrust_input
- THRUST_SET = True
-
-if __name__ == '__main__':
-
- thruster4Data=thrusterData4Thruster()
- thruster6Data=thrusterData6Thruster()
-
- rospy.init_node('surge_control', anonymous=True)
-
- rospy.Subscriber(name=position_topic_name, data_class=positionData, callback=positionCallback)
-
- pub4 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER4, thrusterData4Thruster, queue_size = 2)
- pub6 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, queue_size = 2)
-
- r = rospy.Rate(10)
-
- while not rospy.is_shutdown():
-
- if THRUST_SET:
-
- thruster6Data.data[0] = 0.0
- thruster6Data.data[1] = 0.0
- thruster6Data.data[2] = 0.0
- thruster6Data.data[3] = 0.0
- thruster6Data.data[4] = thrust_input
- thruster6Data.data[5] = thrust_input
-
- thruster4Data.data[0] = thruster6Data.data[0]
- thruster4Data.data[1] = thruster6Data.data[1]
- thruster4Data.data[2] = thruster6Data.data[4]
- thruster4Data.data[3] = thruster6Data.data[5]
-
- #pub4.publish(thruster4Data)
- pub6.publish(thruster6Data)
-
- print "Published data!"
- THRUST_SET = False
-
- r.sleep()
diff --git a/sensor_stack/seabotix/src/test.py b/sensor_stack/seabotix/src/test.py
deleted file mode 100755
index 50d2b6b..0000000
--- a/sensor_stack/seabotix/src/test.py
+++ /dev/null
@@ -1,86 +0,0 @@
-#!/usr/bin/env python
-
-PKG = 'seabotix'
-
-from time import sleep
-import roslib; roslib.load_manifest(PKG)
-import serial
-
-import rospy
-from kraken_msgs.msg import seabotix
-
-dataString = ''
-
-sb = serial.Serial('/dev/ttyACM0', 9600)
-
-#serial config
-sb.stopbits = 1
-
-def initSerial():
- if (sb.isOpen == 0):
- sb.open()
-
- if (sb.isOpen) :
- print 'Serial port opened successfully'
-
- else:
- print 'Error in opening port'
-
-def seabotixCB(data):
- global dataString
- dataString = ''
- checksum = '0x00'
-
- for i in data:
- dataString += chr(data.data[i])
- checksum += data.data[i]
- dataString += chr(checksum)
- sb.write(dataString)
-
-if __name__ == '__main__':
-
- initSerial()
-
- rospy.init_node('Thruster', anonymous=True)
- sub = rospy.Subscriber('/kraken/seabotix', seabotix, seabotixCB)
-
- # count = 0 # variable to check frequency
- # add = [0X60,0X52,0X5A,0X50,0X5C,0X5E]
- # speed = [0X62,0X62,0X62,0X62,0X62,0X62] 5A and 60 for depth and
- # speedMax = [0X64,0X64,0X64,0X64,0X64,0X64] n 52 and 5C are surge thrusters
- data = [[0x60,0xA0,0x64], #depth Back
- [0x52,0xA0,0x64], #Surge Left
- [0x5A,0xA0,0x64], #depth Front
- [0x50,0x80,0x64],
- [0x5C,0xA0,0x64], #Surge Right
- [0x5E,0x62,0x64]]
-
- # add[0] = 50
- # add[1] = '56'
- # add[2] = '5A'
- # add[3] = '5E'
- # add[4] = '52'
- # add[5] = '58'
- # add[4] = '60'
- # add[5] = '5C'
-
- r = rospy.Rate(1)
-
- print 'Started running'
-
- # print speed
-
- # print sb.readline()
- while not rospy.is_shutdown():
- print "Cycle Started"
- for i in range(0,6):
- for j in range(0,3):
- sb.write(str(chr(int(data[i][j]))))
- # print sb.readline()
- print "Single thruster data published"
- print "Cycle Ended"
- # Press Ctrl+C here to stop publishing new data
- r.sleep()
-
- sb.close()
- print "The while loop has ended"
diff --git a/sensor_stack/seabotix/src/testThrusters.py b/sensor_stack/seabotix/src/testThrusters.py
old mode 100755
new mode 100644
index 5565190..b6a55ab
--- a/sensor_stack/seabotix/src/testThrusters.py
+++ b/sensor_stack/seabotix/src/testThrusters.py
@@ -9,6 +9,11 @@
# It does so using a while loop.
+# TODO:
+
+# - Ctrl+C handling
+# - Write a bash script to start this up directly from the repo root.
+
PKG = 'seabotix'
from time import sleep
@@ -21,7 +26,6 @@ from kraken_msgs.msg import seabotix
import os
import signal
import sys
-import time
dataString = ''
@@ -30,26 +34,11 @@ sb = serial.Serial('/dev/ttyACM0', 9600)
# serial config
sb.stopbits = 1
-def stopThrustersNow(signal, frame):
+def stopThrustersNow(s, f):
print "Stopping thrusters now!"
- print os.getcwd()
- data = [[0x60,0x80,0x64], #depth Back
- [0x50,0x80,0x64], #Surge Left
- [0x5A,0x80,0x64], #depth Front
- [0x52,0x80,0x64],
- [0x5C,0x80,0x64], #Surge Right
- [0x5E,0x80,0x64]]
- global sb
- print "Cycle Started"
- for i in range(0,6):
- for j in range(0,3):
- sb.write(chr(data[i][j]))
- print "Thruster",(i+1),"stopped"
-
- sb.close()
- exit()
- # time.sleep(1)
+ os.system("python stopKraken.py")
+ # signal.alarm(2)
# sys.exit(0)
def initSerial():
@@ -65,7 +54,6 @@ def initSerial():
if __name__ == '__main__':
signal.signal(signal.SIGINT, stopThrustersNow)
- # signal.signal(signal.SIGTERM, stopThrustersNow)
initSerial()
rospy.init_node('testThrusters', anonymous=True) # required for using rospy.Rate
@@ -75,14 +63,12 @@ if __name__ == '__main__':
# speed = [0X62,0X62,0X62,0X62,0X62,0X62] 5A and 60 for depth and
# speedMax = [0X64,0X64,0X64,0X64,0X64,0X64] n 52 and 5C are surge thrusters
- data = [
- [0x60,0xAA,0x64], #depth Back
- [0x50,0xAA,0x64], #Surge Left
- [0x5A,0xCD,0x64], #depth Front
- [0x52,0xAA,0x64],
- [0x5C,0xAA,0x64], #Surge Right
- [0x5E,0xAA,0x64]
- ]
+ data = [[0x60,0xAA,0x64], #depth Back
+ [0x52,0xB0,0x64], #Surge Left
+ [0x5A,0xAA,0x64], #depth Front
+ [0x50,0x80,0x64],
+ [0x5C,0xAA,0x64], #Surge Right
+ [0x5E,0x62,0x64]]
# add[0] = 50
# add[1] = '56'
@@ -93,21 +79,17 @@ if __name__ == '__main__':
# add[4] = '60'
# add[5] = '5C'
- r = rospy.Rate(10)
+ r = rospy.Rate(1)
- # print "Entering While Loop"
- #print sb.readline()
+ print "Entering While Loop"
while not rospy.is_shutdown():
- print "Cycle Started :"
+ print "Cycle Started"
for i in range(0,6):
for j in range(0,3):
- sb.write(chr(data[i][j]))
- print "Thruster ",(i+1)
+ sb.write(str(chr(int(data[i][j]))))
+ print "Single Thuster Data-point completed." # Total 18 data-points
+ print "Cycle Ended."
r.sleep()
sb.close()
-
- print "Exited from While Loop!"
-
- # os.system("python src/stopKraken.py")
diff --git a/sensor_stack/seabotix/src/yaw-control-with-twiddle/twiddle.py b/sensor_stack/seabotix/src/yaw-control-with-twiddle/twiddle.py
deleted file mode 100644
index 9debc8b..0000000
--- a/sensor_stack/seabotix/src/yaw-control-with-twiddle/twiddle.py
+++ /dev/null
@@ -1,170 +0,0 @@
-#!/usr/bin/env python
-
-PKG = 'seabotix'
-
-import roslib; roslib.load_manifest(PKG)
-import serial
-import rospy
-import numpy
-from kraken_msgs.msg import thrusterData6Thruster
-from kraken_msgs.msg import thrusterData4Thruster
-from kraken_msgs.msg import imuData
-from kraken_msgs.msg import absoluteRPY
-from resources import topicHeader
-
-import numpy # for the function arctan2
-from math import *
-
-PKG = 'seabotix'
-
-N = 100
-errorP = 0.0
-errorI = 0.
-errorD = 0.
-yaw = 0.0
-goal = 20
-base_yaw = 0.0
-FIRST_ITERATION = True
-imu_data_flag = False
-
-print topicHeader.ABSOLUTE_RPY
-
-def yaw_control(params):
-
- print "entered yaw-control"
-
- global errorI
- global errorP
- global errorD
- global imu_data_flag
-
- def abrpyCB(abrpy):
- global errorI
- global errorP
- global errorD
- global prevError
- global FIRST_ITERATION
- global base_yaw
- global imu_data_flag
-
- # print "Entered ABRPY callback"
-
- # yaw = float(raw_input("Enter yaw: "))
-
- yaw = abrpy.yaw
-
- if FIRST_ITERATION:
-
- base_yaw = yaw
- FIRST_ITERATION = False
-
- prevError = errorP
- # errorP should be four quadrant arc tangent so numpy.arctan2(sin(errorP),cos(errorP))
- errorP = base_yaw + goal - yaw
- errorP = numpy.arctan2(sin(errorP),cos(errorP))
- # print errorP
- errorI = errorP + prevError
- errorD = errorP - prevError
- # print errorP
-
- imu_data_flag = True
-
- rospy.init_node('twiddle_algm_node', anonymous=True)
- sub = rospy.Subscriber(topicHeader.ABSOLUTE_RPY, absoluteRPY, abrpyCB)
- pub4 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER4, thrusterData4Thruster, queue_size = 2)
- pub6 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, queue_size = 2)
-
- Kp = params[0]
- Ki = params[1]
- Kd = params[2]
-
- err = 0.
-
- i = 0
-
- while (i < 2*N):
-
- thruster6Data = thrusterData6Thruster()
- thruster4Data = thrusterData4Thruster()
-
- thruster6Data.data[0] = 0.0
- thruster6Data.data[1] = 0.0
- thruster6Data.data[2] = 0.0
- thruster6Data.data[3] = 0.0
-
- thruster6Data.data[4] = Kp*errorP + Kd*errorD + Ki*errorI
- thruster6Data.data[5] = -1 * thruster6Data.data[4]
-
- thruster4Data.data[0] = thruster6Data.data[0]
- thruster4Data.data[1] = thruster6Data.data[1]
- thruster4Data.data[2] = thruster6Data.data[4]
- thruster4Data.data[3] = thruster6Data.data[5]
-
- # pub4.publish(thruster4Data)
-
- pub6.publish(thruster6Data)
-
- # Run the twiddle loop 200 times
- # the first 100 times only control step will take place
- # and the next 100 times, the error will be noted down.
- if imu_data_flag:
-
- i += 1
-
- # print i
-
- if i >= N:
-
- err += (errorP**2)
-
- # print i, "th loop: Current accumulated error: ", err
-
- imu_data_flag = False
-
- be = err/N;
-
- print "Best error: ", round(be, 2)
- # temp = raw_input("Press enter to continue")
-
- return err / N;
-
-def twiddle(tol = 0.001):
-
- n_params = 3
- dparams = [2.5 for row in range(n_params)]
- params = [0.0 for row in range(n_params)]
-
- best_error = yaw_control(params)
-
- n = 0
-
- while sum(dparams) > tol:
- for i in range(len(params)):
- params[i] += dparams[i]
- print "params[", i, "]: ", params[i]
- err = yaw_control(params)
- if err < best_error:
- best_error = err
- dparams[i] *= 1.1
- else:
- params[i] -= 2.0 * dparams[i]
- err = yaw_control(params)
- if err < best_error:
- best_error = err
- dparams[i] *= 1.1
- else:
- params[i] += dparams[i]
- dparams[i] *= 0.9
- n += 1
-
- print 'Twiddle #', n, params, ' -> ', best_error, ' dparams: ', dparams
-
- temp = raw_input("Press enter to continue")
-
- print ''
- return params
-
-params = twiddle()
-# err = controller.yaw_control(params, True)
-# print '\nFinal parameters: ',params, '\n ->' ,err
-print "Final parameters: ", params
diff --git a/sensor_stack/seabotix/src/yawAndDepthControl.py b/sensor_stack/seabotix/src/yawAndDepthControl.py
deleted file mode 100644
index 6fc8205..0000000
--- a/sensor_stack/seabotix/src/yawAndDepthControl.py
+++ /dev/null
@@ -1,178 +0,0 @@
-#!/usr/bin/env python
-
-PKG = 'seabotix'
-
-import sys
-import roslib; roslib.load_manifest(PKG)
-import serial
-import rospy
-from kraken_msgs.msg import thrusterData6Thruster
-from kraken_msgs.msg import thrusterData4Thruster
-from kraken_msgs.msg import dvlData
-from kraken_msgs.msg import imuData
-from resources import topicHeader
-from dynamic_reconfigure.server import Server
-from seabotix.cfg import k_changerConfig
-
-if len(sys.argv) < 3:
-
- print "Give more arguments!"
- exit()
-
-
-offset = 0.8
-depth = 0.0
-depthGoal = float(sys.argv[2])
-
-yaw = 0.0
-yawGoal = float(sys.argv[1])
-
-Kp_left = 1.27;
-Kd_left = 0.016;
-Ki_left = 0.00;
-
-Kp_right = -1.27;
-Kd_right = -0.016;
-Ki_right = -0.00;
-
-yaw_errorI = 0.0
-yaw_errorP = 0.0
-yaw_errorD = 0.0
-yaw_prevError = 0.0
-
-Kp = 69
-Kd = 27
-Ki = 25
-
-# Use in case of mismatch
-Kp_front = 200;
-Kd_front = 0.5;
-Ki_front = 1;
-
-Kp_back = 200;
-Kd_back = 0.5;
-Ki_back = 1;
-
-depth_errorI = 0.0
-depth_errorP = 0.0
-depth_errorD = 0.0
-depth_prevError = 0.0
-i= 0
-
-depth0 = 0.0
-depth1 = 0.0
-
-setyawnow = False
-setdepthnow = False
-
-def imuCB(dataIn):
- global yaw
- global yaw_errorI
- global yaw_errorP
- global yaw_errorD
- global yaw_prevError
-
- yaw = dataIn.data[2]
-
- yaw_prevError = yaw_errorP
- yaw_errorP = yawGoal - yaw
- yaw_errorI = yaw_errorP + yaw_prevError
- yaw_errorD = yaw_errorP - yaw_prevError
-
- global setyawnow
- setyawnow = True
-
-def dvlCB(dataIn):
- global depth
- global depthGoal
- global offset
- global depth_errorI
- global depth_errorP
- global depth_errorD
- global depth_prevError
- global i
-
- if(i < 3):
- offset = dataIn.data[6]
- i = i+1
- print i,offset
-
- depth = offset - dataIn.data[6]
-
-
- depth_prevError = depth_errorP
- depth_errorP = depthGoal - depth
- print 'Error: ', depth_errorP
- depth_errorI = depth_errorP + depth_prevError
- depth_errorD = depth_errorP - depth_prevError
-
- global setdepthnow
- setdepthnow = True
-
-def depthFeedbackCB(data):
- global depth0
- global depth1
- depth0 = data.data[0]
- depth1 = data.data[1]
-
-thruster6Data = thrusterData6Thruster()
-thruster4Data = thrusterData4Thruster()
-
-def callback(config,level):
- global Kp,Kd,Ki
- Kp=2*config.Kpl
- Kd=config.Kdl
- Ki=config.Kil
- return config
-
-
-if __name__ == '__main__':
-
- rospy.init_node('CompleteControl', anonymous=True)
- sub = rospy.Subscriber(topicHeader.SENSOR_DVL, dvlData, dvlCB)
- sub1 = rospy.Subscriber(topicHeader.SENSOR_IMU, imuData, imuCB)
- pub4 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER4, thrusterData4Thruster, queue_size = 2)
- pub6 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, queue_size = 2)
- srv=Server(k_changerConfig,callback)
- sub2 = rospy.Subscriber(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, depthFeedbackCB)
-
- r = rospy.Rate(10)
-
- while not rospy.is_shutdown():
-
- #print Kp,Kd,Ki,errorP,errorD,errorI
- #print thruster6Data.data[0]
-
- thruster6Data.data[0] = depth0
- thruster6Data.data[1] = depth1
- thruster6Data.data[4] = 0.0
- thruster6Data.data[5] = 0.0
-
- if setdepthnow:
- thruster6Data.data[0] = Kp*depth_errorP + Kd*depth_errorD + Ki*depth_errorI
- thruster6Data.data[1] = Kp*depth_errorP + Kd*depth_errorD + Ki*depth_errorI
- setdepthnow = False
- print "Setting depth now!", depth0, " : ", depth1
-
- if setyawnow:
- thruster6Data.data[4] = Kp_left*yaw_errorP + Kd_left*yaw_errorD + Ki_left*yaw_errorI
- thruster6Data.data[5] = Kp_right*yaw_errorP + Kd_right*yaw_errorD + Ki_right*yaw_errorI
- setyawnow = False
- print "Setting yaw now!"
-
- thruster6Data.data[2] = 0.0
- thruster6Data.data[3] = 0.0
-
- thruster6Data.data[0] = 4
- thruster6Data.data[1] = 4
-
- thruster4Data.data[0] = thruster6Data.data[0]
- thruster4Data.data[1] = thruster6Data.data[1]
- thruster4Data.data[2] = thruster6Data.data[4]
- thruster4Data.data[3] = thruster6Data.data[5]
-
- #pub4.publish(thruster4Data)
- pub6.publish(thruster6Data)
-
-
- r.sleep()
diff --git a/sensor_stack/seabotix/src/yawControlOnVehicle.py b/sensor_stack/seabotix/src/yawControlOnVehicle.py
deleted file mode 100755
index 22b7e6e..0000000
--- a/sensor_stack/seabotix/src/yawControlOnVehicle.py
+++ /dev/null
@@ -1,105 +0,0 @@
-#!/usr/bin/env python
-
-PKG = 'seabotix'
-
-import roslib; roslib.load_manifest(PKG)
-import serial
-import rospy
-from kraken_msgs.msg import thrusterData6Thruster
-from kraken_msgs.msg import thrusterData4Thruster
-from kraken_msgs.msg import absoluteRPY
-from resources import topicHeader
-
-import sys
-
-import numpy
-from math import *
-# Yaw must be passed as the first argument to the command
-
-if(len(sys.argv) < 2):
-
- print "Enter yaw, to run this script."
- exit()
-
-yaw = 0.0
-goal = float(sys.argv[1])
-
-base_yaw = 0.0
-FIRST_ITERATION = True
-factor=0.5
-Kp_left = 0.95
-Kd_left = 0.035
-Ki_left = 0.0005
-
-Kp_right = -0.95
-Kd_right = -0.035
-Ki_right = -0.0005
-
-# Kp_left = 1.27;
-# Kd_left = 0.046;
-# Ki_left = 0.00;
-
-# Kp_right = -1.27;
-# Kd_right = -0.016;
-# Ki_right = -0.00;
-
-errorI = 0.0
-errorP = 0.0
-errorD = 0.0
-prevError = 0.0
-
-def abrpyCB(abrpy):
-
- global yaw
- global errorI
- global errorP
- global errorD
- global prevError
- global FIRST_ITERATION
- global base_yaw
-
- yaw = abrpy.yaw
-
- if FIRST_ITERATION:
-
- base_yaw = yaw
- FIRST_ITERATION = False
-
- prevError = errorP
- errorP = base_yaw + goal - yaw
- errorP = errorP * 3.14 / 180
- errorP = numpy.arctan2(sin(errorP),cos(errorP))
- errorP = errorP * 180 / 3.14
- print "yaw: ", round(yaw, 2), "errorP: ", round(errorP, 2)
- errorI = errorP + errorI
- errorD = errorP - prevError
-
-if __name__ == '__main__':
- thruster4Data=thrusterData4Thruster();
- thruster6Data=thrusterData6Thruster();
-
- rospy.init_node('yaw_control_node', anonymous=True)
- sub = rospy.Subscriber(topicHeader.ABSOLUTE_RPY, absoluteRPY, abrpyCB)
- pub4 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER4, thrusterData4Thruster, queue_size = 2)
- pub6 = rospy.Publisher(topicHeader.CONTROL_PID_THRUSTER6, thrusterData6Thruster, queue_size = 2)
-
- r = rospy.Rate(10)
-
- while not rospy.is_shutdown():
-
- thruster6Data.data[0] = 0.0
- thruster6Data.data[1] = 0.0
- thruster6Data.data[2] = 0.0
- thruster6Data.data[3] = 0.0
- thruster6Data.data[4] = Kp_left*errorP + Kd_left*errorD + Ki_left*errorI
- thruster6Data.data[5] = (Kp_right*errorP + Kd_right*errorD + Ki_right*errorI)
-
- thruster4Data.data[0] = thruster6Data.data[0]
- thruster4Data.data[1] = thruster6Data.data[1]
- thruster4Data.data[2] = thruster6Data.data[4]
- thruster4Data.data[3] = thruster6Data.data[5]
-
- pub6.publish(thruster6Data)
-
-
- r.sleep()
diff --git a/sensor_stack/sparton/src/sparton.py b/sensor_stack/sparton/src/sparton.py
old mode 100755
new mode 100644
diff --git a/simulator_stack/simulator_to_navpose/src/main.cpp b/simulator_stack/simulator_to_navpose/src/main.cpp
index bbb7077..864919a 100644
--- a/simulator_stack/simulator_to_navpose/src/main.cpp
+++ b/simulator_stack/simulator_to_navpose/src/main.cpp
@@ -7,8 +7,8 @@
#include "underwater_sensor_msgs/DVL.h"
#include "underwater_sensor_msgs/Pressure.h"
#include "kraken_msgs/krakenPose.h"
-#include "geometry_msgs/PoseStamped.h".h"
-#include "geometry_msgs/TwistStamped.h".h"
+#include "geometry_msgs/PoseStamped.h"
+#include "geometry_msgs/TwistStamped.h"
#include "pose_server/KrakenPose.h"
const double PI=3.14;
class NavPub
diff --git a/vision_stack/task_buoy/include/task_buoy/buoy_server.h b/vision_stack/task_buoy/include/task_buoy/buoy_server.h
index f0c573f..5c022de 100644
--- a/vision_stack/task_buoy/include/task_buoy/buoy_server.h
+++ b/vision_stack/task_buoy/include/task_buoy/buoy_server.h
@@ -37,13 +37,9 @@ private:
image_transport::Publisher _pub;
actionmsg::buoyFeedback _feedback;
actionmsg::buoyResult _result;
-
- ros::Publisher _buoy_coord_pub;
- ros::NodeHandle _nh;
-
Mat _image, _imageHSV, _imageBW, _imageBWRed, _imageBWGreen;
Scalar _lowerThreshRed1, _lowerThreshRed2, _upperThreshRed1, _upperThreshRed2;
- Scalar _lowerThreshGreen, _upperThreshGreen, _lowerThreshYellow, _upperThreshYellow;
+ Scalar _lowerThreshGreen, _upperThreshGreen;
Mat _kernelDilateErode;
std::string _actionName;
cv_bridge::CvImage _finalImage;
@@ -52,11 +48,6 @@ private:
vector<vector<Point> > _contoursPoly;
vector<Point2f> _center;
vector<float> _radius;
- vector<Vec3f> circles;
- vector<int> _fradius;
- vector<Point> _fcenter;
- int max ;
- int index;
public:
Buoy(std::string _name);
void executeCB(const actionmsg::buoyGoalConstPtr &_goal);
@@ -69,4 +60,3 @@ public:
};
#endif
-
diff --git a/vision_stack/task_buoy/manifest.xml b/vision_stack/task_buoy/manifest.xml
index 0b4cf2a..c62230f 100644
--- a/vision_stack/task_buoy/manifest.xml
+++ b/vision_stack/task_buoy/manifest.xml
@@ -15,7 +15,6 @@
<depend package="image_transport"/>
<depend package="cv_bridge"/>
<depend package="resources"/>
- <depend package="kraken_msgs"/>
</package>
diff --git a/vision_stack/task_buoy/src/buoy_client.cpp b/vision_stack/task_buoy/src/buoy_client.cpp
index d38b8bd..e0ce6eb 100644
--- a/vision_stack/task_buoy/src/buoy_client.cpp
+++ b/vision_stack/task_buoy/src/buoy_client.cpp
@@ -18,7 +18,7 @@ int main(int argc, char ** argv)
_goal.order = DETECT_BUOY;
ROS_INFO("Sending goal - DETECT_BUOY.");
_client.sendGoal(_goal);
- bool _actionStatus = _client.waitForResult(ros::Duration(3000.0));
+ bool _actionStatus = _client.waitForResult(ros::Duration(300.0));
if(_actionStatus == true)
{
@@ -34,7 +34,7 @@ int main(int argc, char ** argv)
ROS_INFO("Sending goal - ALIGN_BUOY");
_goal.order = ALIGN_BUOY;
_client.sendGoal(_goal);
- _actionStatus = _client.waitForResult(ros::Duration(3000.0));
+ _actionStatus = _client.waitForResult(ros::Duration(300.0));
if(_actionStatus == true)
{
diff --git a/vision_stack/task_buoy/src/buoy_server.cpp b/vision_stack/task_buoy/src/buoy_server.cpp
index 690fb22..84de24a 100644
--- a/vision_stack/task_buoy/src/buoy_server.cpp
+++ b/vision_stack/task_buoy/src/buoy_server.cpp
@@ -4,186 +4,43 @@
#include <resources/topicHeader.h>
-#include <kraken_msgs/positionData.h>
-
-
-/* white balance code starts here*/
-
-
-class maxRGB
-{
-public:
- Mat run(Mat,int p,int m);
- void process(Mat src1,float *ml,float *ma,float *mb,int p,int m);
-};
-
-maxRGB obj;
-
-
-Mat maxRGB::run(Mat src1,int p,int m)
+Buoy::Buoy(std::string name) : _it(_n), _s(_n, name, boost::bind(&Buoy::executeCB, this, _1), false), _actionName(name)
{
- Mat dst;
-
- src1.copyTo(dst);
-
- cv::Mat_<cv::Vec3b>::const_iterator it= src1.begin<cv::Vec3b>();
- cv::Mat_<cv::Vec3b>::const_iterator itend= src1.end<cv::Vec3b>();
- cv::Mat_<cv::Vec3b>::iterator itout= dst.begin<cv::Vec3b>();
-
- float ma=0,mb=0,ml=0;
- process(src1,&ml,&ma,&mb,p,m);
+ _sub = _it.subscribe(topics::CAMERA_FRONT_RAW_IMAGE, 1, &Buoy::imageCallBack, this);
+ _pub = _it.advertise(topics::CAMERA_FRONT_BUOY_IMAGE, 1);
+ ifstream _thresholdVal("threshold.th");
- for ( ; it!= itend; ++it, ++itout)
+ if(_thresholdVal.is_open())
{
-
-
- cv::Vec3b v1=*it;
-
- float l=v1.val[0];
- float a=v1.val[1];
- float b=v1.val[2];
-
- if(m==0)
- {
- a=a*(ma);
- b= b*(mb);
- l= l*(ml);
- }
- else
- {
- //if(a<(float)95*255/100)
- a=a*(ma);
- //if(b<(float)95*255/100)
- b= b*(mb);
- // if(l<(float)95*255/100)
- l= l*(ml);
- }
-
- if(a>255)
- {
- a=255;
- }
-
- if(b>255)
- {
- b=255;
- }
-
- if(l>255)
+ for(int i = 0; i < 3; i++)
{
- l=255;
+ _thresholdVal >> _lowerThreshRed1[i];
}
-
- v1.val[0]=l;
-
- v1.val[1]=a;
-
- v1.val[2]=b;
-
- *itout=v1;
-
-
-
- }
-
- return dst;
-}
-
-
-void maxRGB::process(Mat src1,float *ml,float *ma,float *mb,int p,int m)
-{
-
- Mat src;
- src1.convertTo(src,CV_32FC3,1.0,0);
- *ma=0;
- *mb=0;
- *ml=0;
-
- for(int i=0; i<src.rows; i++)
- {
-
- for(int j=0; j<src.cols; j++)
+ for(int i = 0; i < 3; i++)
{
- Vec3f v1=src.at<cv::Vec3f>(i,j);
- double lc=v1.val[0];
- double ac=v1.val[1];
- double bc=v1.val[2];
- *ml=max((double)*ml,(double)lc);
- *ma=max((double)*ma,(double)ac);
- *mb=max((double)*mb,bc);
-
+ _thresholdVal >> _upperThreshRed1[i];
}
- }
-
- if(m==1)
- {
- double r=(*ma+*mb+*ml)/3;
- r=(double)sqrt((double)(*ma)*(*ma)+(double)(*mb)*(*mb)+(double)(*ml)*(*ml));
- *ma=(double)(*ma)/(double)r;
- *mb=(double)(*mb)/(double)r;
- *ml=(double)(*ml)/(double)r;
-
- r=max((double)*ma,(double)*mb);
- r=max((double)r,(double)*ml);
-
- *ma=(double)(r)/(double)*ma;
- *mb=(double)(r)/(double)*mb;
- *ml=(double)(r)/(double)*ml;
- }
- else
- {
- cerr << *ml << endl;
- cerr << *ma << endl;
- cerr << *mb << endl;
-
- double r=max((double)*ml,(double)*ma);
- r=max((double)r,(double)*mb);
- *ma=(double)(r)/(double)*ma;
- *mb=(double)(r)/(double)*mb;
- *ml=(double)(r)/(double)*ml;
- }
-}
-
-Buoy::Buoy(std::string name) : _it(_n), _s(_n, name, boost::bind(&Buoy::executeCB, this, _1), false), _actionName(name)
-{
- _sub = _it.subscribe(topics::CAMERA_FRONT_RAW_IMAGE, 1, &Buoy::imageCallBack, this);
- _pub = _it.advertise(topics::CAMERA_FRONT_BUOY_IMAGE, 1);
-
- _buoy_coord_pub = _nh.advertise<kraken_msgs::positionData>("/kraken/buoy/coordinates/", 1);
-
- max = 0;
- ifstream _thresholdVal("threshold.th");
-
- if(_thresholdVal.is_open())
- {
for(int i = 0; i < 3; i++)
{
- _thresholdVal >> _lowerThreshYellow[i];
+ _thresholdVal >> _lowerThreshRed2[i];
}
for(int i = 0; i < 3; i++)
{
- _thresholdVal >> _upperThreshYellow[i];
+ _thresholdVal >> _upperThreshRed2[i];
}
- // for(int i = 0; i < 3; i++)
- // {
- // _thresholdVal >> _lowerThreshRed2[i];
- // }
- // for(int i = 0; i < 3; i++)
- // {
- // _thresholdVal >> _upperThreshRed2[i];
- // }
- /*for(int i = 0; i < 3; i++)
+ for(int i = 0; i < 3; i++)
{
_thresholdVal >> _lowerThreshGreen[i];
}
+
for(int i = 0; i < 3; i++)
{
_thresholdVal >> _upperThreshGreen[i];
- }*/
+ }
}
else
{
@@ -191,7 +48,6 @@ Buoy::Buoy(std::string name) : _it(_n), _s(_n, name, boost::bind(&Buoy::executeC
ros::shutdown();
}
- cout << "threshold files read " << endl;
_kernelDilateErode = getStructuringElement(MORPH_RECT, Size(3,3));
_finalImage.encoding = "mono8";
_s.start();
@@ -218,8 +74,6 @@ void Buoy::executeCB(const actionmsg::buoyGoalConstPtr &_goal)
ros::Rate looprate(10);
bool success = true;
- cout <<"<<<<<< _goal->order = >>>>> " << _goal-> order;
-
switch(_goal->order)
{
case DETECT_BUOY:
@@ -244,13 +98,8 @@ void Buoy::executeCB(const actionmsg::buoyGoalConstPtr &_goal)
if(_detected)
{
_result.sequence.push_back(BUOY_DETECTED);
- cout << "<<<<<<<<<< buoy has been detected.. >>>>>>>>>>>>>";
break;
}
- else
- {
- cout << "buoy not detected... " << endl;
- }
looprate.sleep();
}
@@ -275,12 +124,13 @@ void Buoy::executeCB(const actionmsg::buoyGoalConstPtr &_goal)
_finalImage.image = _imageBW;
_finalImagemsg = _finalImage.toImageMsg();
_pub.publish(_finalImagemsg);
- /*if((_feedback.errorx < 10 && _feedback.errorx > -10) && (_feedback.errory > -10 && _feedback.errory < 10))
+
+ if((_feedback.errorx < 5 && _feedback.errorx > -5) && (_feedback.errory > -5 && _feedback.errory < 5))
{
_result.sequence.push_back(BUOY_ALIGNED);
- cout << "<<<<<<<<<<<< BUOY_ALIGNED >>>>>>>>>>>>>>" << endl;
break;
- }*/
+ }
+
_s.publishFeedback(_feedback);
looprate.sleep();
}
@@ -292,60 +142,30 @@ void Buoy::executeCB(const actionmsg::buoyGoalConstPtr &_goal)
if(success)
{
_result.sequence.push_back(_goal->order);
- kraken_msgs::positionData temp;
- // temp.x_coordinate = -1 * _feedback.errorx;
- //temp.y_coordinate = _feedback.errory;
- //_buoy_coord_pub.publish(temp);
-
_s.setSucceeded(_result);
}
}
-
bool Buoy::detectBuoy()
{
if(!_image.empty())
{
- Mat org,_finalImg, gray, edges,dst, org1;
- _finalImg = obj.run(_image,1,0);
-// imshow("max edge",obj.run(_image,1,0));
-// cvtColor(_finalImg, _imageHSV, CV_BGR2HSV);
-// Mat gray,dst;
- imshow("final image", _finalImg);
- cvtColor(_finalImg,gray,CV_BGR2GRAY);
- adaptiveThreshold(gray,org,100,ADAPTIVE_THRESH_MEAN_C,THRESH_BINARY,13,0);
-
-// fastNlMeansDenoising(org,dst,3,7,21 );
-// fastNlMeansDenoising(org, dst);
-// imshow("denoised ", dst);
-// inRange(_imageHSV,_lowerThreshYellow,_upperThreshYellow, org);
- // inRange(_imageHSV,_lowerThreshGreen,_upperThreshGreen, _imageBWGreen);
-
-// imshow("org", org);
-
- Mat org2, erodeimg;
-// medianBlur(org, _imageBW, 3);
-
- erode(org, erodeimg, _kernelDilateErode);
- erode(erodeimg, erodeimg, _kernelDilateErode);
- // imshow("erode", erodeimg);
- dilate(erodeimg, org2, _kernelDilateErode);
-// dilate(org2, org2, _kernelDilateErode);
- Canny(org2,edges,0,100);
-// imshow("eroded image", edges);
- // imshow("dilated ", org2);
- // imshow("canny", edges);
+ cvtColor(_image, _imageHSV, CV_BGR2HSV);
+ inRange(_imageHSV,_lowerThreshRed1,_upperThreshRed1, _imageBW);
+ inRange(_imageHSV,_lowerThreshRed2,_upperThreshRed2, _imageBWRed);
+ add(_imageBW, _imageBWRed, _imageBW);
+ inRange(_imageHSV,_lowerThreshGreen,_upperThreshGreen, _imageBWGreen);
+ add(_imageBW, _imageBWGreen, _imageBW);
+ imshow("_imageBW", _imageBW);
waitKey(33);
+ medianBlur(_imageBW, _imageBW, 3);
+ erode(_imageBW, _imageBW, _kernelDilateErode);
CBlobResult _blobs,_blobsClutter;
CBlob * _currentBlob;
- IplImage _imageBWipl = org2;
+ IplImage _imageBWipl = _imageBW;
_blobs = CBlobResult(&_imageBWipl, NULL, 0);
- _blobs.Filter(_blobs, B_INCLUDE, CBlobGetArea(), B_INSIDE, 200, 5000);
-
-// _imageBW = Scalar(0, 0, 0);
- org2 = Scalar(0, 0, 0);
-
- cout << "number of blobs " << _blobs.GetNumBlobs() << endl;
+ _blobs.Filter(_blobs, B_INCLUDE, CBlobGetArea(), B_INSIDE, 50, 1000);
+ _imageBW = Scalar(0, 0, 0);
for(int i = 0; i < _blobs.GetNumBlobs(); i++)
{
@@ -353,30 +173,22 @@ bool Buoy::detectBuoy()
_currentBlob->FillBlob(&_imageBWipl, Scalar(255));
}
- Mat newimg;
-// Mat _imageBW2 = _imageBW.clone();
- Mat _imageBW2 = org2.clone();
- dilate(_imageBW2, newimg, _kernelDilateErode);
- // imshow("newimg", newimg);
+ Mat _imageBW2 = _imageBW.clone();
Mat src;
vector<Mat> channels;
- channels.push_back(org2);
- channels.push_back(org2);
- channels.push_back(org2);
+ channels.push_back(_imageBW);
+ channels.push_back(_imageBW);
+ channels.push_back(_imageBW);
merge( channels, src);
-
_contours.clear();
-// medianBlur(_imageBW2, _imageBW2, 5);
- // imshow("imageBW2", _imageBW2);
+ medianBlur(_imageBW2, _imageBW2, 5);
findContours(_imageBW2, _contours, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
-
Point2f _centerBuff;
float _radiusBuff;
vector<Point> _contoursPolyBuff;
_center.clear();
_radius.clear();
_contoursPoly.clear();
-
_imageBW = Scalar(0, 0, 0);
for(int i=0; i < _contours.size(); i++)
@@ -395,111 +207,53 @@ bool Buoy::detectBuoy()
Mat src_gray;
cvtColor( src, src_gray, CV_BGR2GRAY );
src = Scalar(0, 0, 0);
-
GaussianBlur( src_gray, src_gray, Size(9, 9), 2, 2 );
- // imshow("src_gray", src_gray);
-
+ imshow("src_gray", src_gray);
+ vector<Vec3f> circles;
/// Apply the Hough Transform to find the circles
- HoughCircles( src_gray, circles, CV_HOUGH_GRADIENT, 1, 80, 180, 25, 30, 300 );
+ HoughCircles( src_gray, circles, CV_HOUGH_GRADIENT, 1, src_gray.rows/16, 150, 25, 0, 0 );
- // /// Draw the circles detected
+ /// Draw the circles detected
if(circles.size() == 0)
{
cout<<"NOTHING CIRCULAR" << endl;
- return false;
}
-
-
-
-
for( size_t i = 0; i < circles.size(); i++ )
{
- cout << "circle wala for loop " << endl;
- cout << "circle area = " << endl;
Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
int radius = cvRound(circles[i][2]);
- _fcenter.push_back(center);
- _fradius.push_back(cvRound(circles[i][2]));
// circle center
circle( src, center, 3, Scalar(0, 255, 0), 3, 8, 0 );
// circle outline
circle( src, center, radius, Scalar(0, 0, 255), 1, 8, 0 );
}
+ imshow("src", src);
- for (int i = 0; i < _fradius.size(); ++i)
- {
- if(_fradius[i] > max)
- {
- index = i;
- max = _fradius[i];
- }
- }
-
- cout << "<<<<<<<<<< largest radius = " << max << " >>>>>>>>>>>>>>>"<<endl;
-
- if(max == 0)
+ if(_center.size() > 0)
{
- cout << "max = 0" << endl;
- return false;
+ return true;
}
else
{
- circle(src,_fcenter[index],3,Scalar(255,255,0),1,8,0);
- circle(src,_fcenter[index], max,Scalar(0,255,0),1,8,0);
-// return true;
+ return false;
}
-
- cout << "show src image" << endl;
- imshow("src", src);
-
- _fcenter.clear();
- _fradius.clear();
-// if(_center.size() > 0){
-// cout << "center size > 0 " << endl;
-// return true;
-// }
-// else{
-// cout << "returns false" << endl;
-// return false;
-// }
-
- return true;
}
else
{
- cout << "no image loaded.." << endl;
return false;
}
-
-
- waitKey(33);
- return 0;
}
-
void Buoy::getAllignment()
{
- cout<<_center.size()<<endl;
-// for(int i = 0; i < _center.size(); i++)
- // {
- _feedback.errorx = _center[index].x-_image.cols/2;
- _feedback.errory = _image.rows/2 - _center[index].y;
- /*if (i>=1)
+ for(int i = 0; i < _center.size(); i++)
{
- _feedback.errory=_feedback.errory<=_center[i].y?_feedback.errory:_image.rows/2 -_center[i].y;
- _feedback.errorx=_feedback.errory<=_center[i].y?_feedback.errorx:_image.cols/2 -_center[i].x;
+ _feedback.errorx = _image.cols/2 - _center[i].x;
+ _feedback.errory = _image.rows/2 - _center[i].y;
+ cout<< _feedback.errorx << " : " << _feedback.errory << endl;
}
- }*/
- cout << "errorx and errory respectively : " ;
- cout<< _feedback.errorx << " and " << _feedback.errory << endl;
-
- kraken_msgs::positionData temp;
- temp.x_coordinate = _feedback.errorx;
- temp.y_coordinate = _feedback.errory;
- _buoy_coord_pub.publish(temp);
-
}
Buoy::~Buoy()
@@ -509,9 +263,7 @@ Buoy::~Buoy()
int main(int argc, char ** argv)
{
ros::init(argc, argv, "buoy_server");
- cout << "running main " <<endl;
Buoy _buoyserver("buoy");
- cout << "node made.. " << endl;
ros::spin();
return 0;
}
diff --git a/vision_stack/task_buoy/src/newbuoy.cpp b/vision_stack/task_buoy/src/newbuoy.cpp
deleted file mode 100644
index 742c1d7..0000000
--- a/vision_stack/task_buoy/src/newbuoy.cpp
+++ /dev/null
@@ -1,623 +0,0 @@
-#include <ros/ros.h>
-#include <task_buoy/buoy_server.h>
-#define PI 3.1414
-
-#include <resources/topicHeader.h>
-
-
-/* white balance code starts here
-
-
-*/
-
-
-
-
-class max_edge
-{
-public:
- /** main function to call to perform max-edge color correction */
- Mat run(Mat,int p,int m);
-
- /** defines convolution type **/
- enum ConvolutionType
- {
- /* Return the full convolution, including border */
- CONVOLUTION_FULL,
-
- /* Return only the part that corresponds to the original image */
- CONVOLUTION_SAME,
-
- /* Return only the submatrix containing elements that were not influenced by the border */
- CONVOLUTION_VALID
- };
-
- /** method to perform 2D convolution */
- void conv2(const Mat &img, const Mat& kernel, ConvolutionType type, Mat& dest);
-
- /** function that computes illumination vector for max_edge algorithm**/
- void process(Mat src1,float *ml,float *ma,float *mb,int p,int m);
-} obj1;
-
-
-
-
-Mat color_correction::max_edge::run( Mat src1,int p,int m)
-{
-
- Mat dst;
-
- src1.copyTo(dst);
-
- cv::Mat_<cv::Vec3b>::const_iterator it= src1.begin<cv::Vec3b>();
- cv::Mat_<cv::Vec3b>::const_iterator itend= src1.end<cv::Vec3b>();
- cv::Mat_<cv::Vec3b>::iterator itout= dst.begin<cv::Vec3b>();
-
- float ma=0,mb=0,ml=0;
- process(src1,&ml,&ma,&mb,p,m);
-
- for ( ; it!= itend; ++it, ++itout)
- {
-
-
- cv::Vec3b v1=*it;
-
- float l=v1.val[0];
- float a=v1.val[1];
- float b=v1.val[2];
-
- if(m==0)
- {
- a=a*(ma);
- b= b*(mb);
- l= l*(ml);
- }
- else
- {
- //if(a<(float)95*255/100)
- a=a*(ma);
- //if(b<(float)95*255/100)
- b= b*(mb);
- // if(l<(float)95*255/100)
- l= l*(ml);
- }
-
- if(a>255)
- {
- a=255;
- }
-
- if(b>255)
- {
- b=255;
- }
-
- if(l>255)
- {
- l=255;
- }
-
-
- v1.val[0]=l;
-
- v1.val[1]=a;
-
- v1.val[2]=b;
-
- *itout=v1;
-
-
-
- }
-
- return dst;
-}
-
-
-
-void color_correction::max_edge::conv2(const Mat &img, const Mat& kernel, ConvolutionType type, Mat& dest)
-{
- Mat source = img;
-
- if(CONVOLUTION_FULL == type)
- {
- source = Mat();
- const int additionalRows = kernel.rows-1, additionalCols = kernel.cols-1;
- copyMakeBorder(img, source, (additionalRows+1)/2, additionalRows/2, (additionalCols+1)/2, additionalCols/2, BORDER_CONSTANT, Scalar(0));
- }
-
- Point anchor(kernel.cols - kernel.cols/2 - 1, kernel.rows - kernel.rows/2 - 1);
- int borderMode = BORDER_CONSTANT;
- flip(kernel,kernel,-1);
- filter2D(source, dest, img.depth(),kernel, anchor, 0, borderMode);
-
- if(CONVOLUTION_VALID == type)
- {
- dest = dest.colRange((kernel.cols-1)/2, dest.cols - kernel.cols/2)
- .rowRange((kernel.rows-1)/2, dest.rows - kernel.rows/2);
- }
-}
-
-
-void color_correction::max_edge::process(Mat src1,float *ml,float *ma,float *mb,int p,int m)
-{
-
- *ma=0;
- *mb=0;
- *ml=0;
-
-
- Mat src;
- vector<Mat> bgr_planes;
- Mat src2;
- //src1.convertTo(src2,CV_32FC3,1.0/255.0,0);
- src1.convertTo(src,CV_32FC3,1.0,0);
- split( src, bgr_planes );
-
- Mat kernel=getGaussianKernel(100,6,CV_32F);
-
-
- Mat grad_x, grad_y,abs_angle_x;
- Mat grad1,grad2,grad3;
- Mat r1,g,b;
-
- cerr << bgr_planes[0].depth() << ":" << CV_32FC1<< "dddddd" << endl;
-
- conv2( bgr_planes[0], kernel, CONVOLUTION_FULL,r1);
- conv2( bgr_planes[1], kernel, CONVOLUTION_FULL,g);
- conv2( bgr_planes[2], kernel, CONVOLUTION_FULL,b);
-
- //cv::imshow("dddd",b);
- // 1 2 1 1 0 -1
- // 0 0 0 2 0 -2
- // -1 -2 -1 1 0 -1
- double dx[]= {1,2,1,0,0,0,-1,-2,-1};
- double dy[]= {1,0,-1,2,0,-2,1,0,-1};
-
- Mat kx=Mat(3,3,CV_32FC1,dx);
- Mat ky=Mat(3,3,CV_32FC1,dy);
-
-
- grad_x.setTo(cv::Scalar::all(0));
- grad_y.setTo(cv::Scalar::all(0));
-
- conv2(r1, kx, CONVOLUTION_FULL,grad_x);
- conv2(r1, ky, CONVOLUTION_FULL,grad_y);
-
-
- cartToPolar(grad_x,grad_y,bgr_planes[0],abs_angle_x,false);
- grad_x.setTo(cv::Scalar::all(0));
- grad_y.setTo(cv::Scalar::all(0));
-
- conv2(g, kx, CONVOLUTION_FULL,grad_x);
- conv2(g, ky, CONVOLUTION_FULL,grad_y);
-
- cartToPolar(grad_x,grad_y,bgr_planes[1],abs_angle_x,false);
- grad_x.setTo(cv::Scalar::all(0));
- grad_y.setTo(cv::Scalar::all(0));
-
- conv2(b, kx, CONVOLUTION_FULL,grad_x);
- conv2(b, ky, CONVOLUTION_FULL,grad_y);
-
- cartToPolar(grad_x,grad_y,bgr_planes[2],abs_angle_x,false);
-
-
-
-
-
-
-
- cv::merge(bgr_planes,src);
-
- for(int i=0; i<src.rows; i++)
- {
-
- for(int j=0; j<src.cols; j++)
- {
- Vec3f v1=src.at<cv::Vec3f>(i,j);
- double lc=v1.val[0];
- double ac=v1.val[1];
- double bc=v1.val[2];
-// *ma=*ma+ac;
-// *mb=*mb+bc;
-// *ml=*ml+lc;
- //cerr << lc << ":" ;
- *ml=max((double)*ml,(double)lc);
- *ma=max((double)*ma,(double)ac);
- *mb=max((double)*mb,(double)bc);
- }
- }
-
-
-// *ma=pow((double)*ma,(double)1/p);
-// *mb=pow((double)*mb,(double)1/p);
-// *ml=pow((double)*ml,(double)1/p);
-
-
- double r=0;
-
- if(m==0)
- {
- r=(*ma+*mb+*ml)/3;
-
- *ma=r/(*ma);
- *mb=r/(*mb);
- *ml=r/(*ml);
- cerr << *ml << endl;
- cerr << *ma << endl;
- cerr << *mb << endl;
-
- }
-
- if(m==1)
- {
- r=(*ma+*mb+*ml)/3;
- r=max((double)*ma,(double)*mb);
- r=max((double)r,(double)*ml);
-
- *ma=r/(*ma);
- *mb=r/(*mb);
- *ml=r/(*ml);
- cerr << *ml << endl;
- cerr << *ma << endl;
- cerr << *mb << endl;
- }
-
- if(m==2)
- {
- r=sqrt((*ma)*(*ma)+(*mb)*(*mb)+(*ml)*(*ml));
- *ma=(double)(*ma)/(double)r;
- *mb=(double)(*mb)/(double)r;
- *ml=(double)(*ml)/(double)r;
-
- cerr << *ml << endl;
- cerr << *ma << endl;
- cerr << *mb << endl;
-
- r=max((double)*ma,(double)*mb);
- r=max((double)r,(double)*ml);
-
- *ma=sqrt((double)r/(double)(*ma));
- *mb=sqrt((double)r/(double)(*mb));
- *ml=sqrt((double)r/(double)(*ml));
- }
-
-
-
-
-}
-
-
-
-
-
-
-
-class maxRGB
-{
-public:
- Mat run(Mat,int p,int m);
- void process(Mat src1,float *ml,float *ma,float *mb,int p,int m);
-};
-
-maxRGB obj;
-
-
-Mat maxRGB::run(Mat src1,int p,int m)
-{
- Mat dst;
-
- src1.copyTo(dst);
-
- cv::Mat_<cv::Vec3b>::const_iterator it= src1.begin<cv::Vec3b>();
- cv::Mat_<cv::Vec3b>::const_iterator itend= src1.end<cv::Vec3b>();
- cv::Mat_<cv::Vec3b>::iterator itout= dst.begin<cv::Vec3b>();
-
- float ma=0,mb=0,ml=0;
- process(src1,&ml,&ma,&mb,p,m);
-
- for ( ; it!= itend; ++it, ++itout)
- {
-
-
- cv::Vec3b v1=*it;
-
- float l=v1.val[0];
- float a=v1.val[1];
- float b=v1.val[2];
-
- if(m==0)
- {
- a=a*(ma);
- b= b*(mb);
- l= l*(ml);
- }
- else
- {
- //if(a<(float)95*255/100)
- a=a*(ma);
- //if(b<(float)95*255/100)
- b= b*(mb);
- // if(l<(float)95*255/100)
- l= l*(ml);
- }
-
- if(a>255)
- {
- a=255;
- }
-
- if(b>255)
- {
- b=255;
- }
-
- if(l>255)
- {
- l=255;
- }
-
-
- v1.val[0]=l;
-
- v1.val[1]=a;
-
- v1.val[2]=b;
-
- *itout=v1;
-
-
-
- }
-
- return dst;
-}
-
-
-void maxRGB::process(Mat src1,float *ml,float *ma,float *mb,int p,int m)
-{
-
- Mat src;
- src1.convertTo(src,CV_32FC3,1.0,0);
- *ma=0;
- *mb=0;
- *ml=0;
-
- for(int i=0; i<src.rows; i++)
- {
-
- for(int j=0; j<src.cols; j++)
- {
- Vec3f v1=src.at<cv::Vec3f>(i,j);
- double lc=v1.val[0];
- double ac=v1.val[1];
- double bc=v1.val[2];
- *ml=max((double)*ml,(double)lc);
- *ma=max((double)*ma,(double)ac);
- *mb=max((double)*mb,bc);
-
- }
- }
-
- if(m==1)
- {
- double r=(*ma+*mb+*ml)/3;
- r=(double)sqrt((double)(*ma)*(*ma)+(double)(*mb)*(*mb)+(double)(*ml)*(*ml));
- *ma=(double)(*ma)/(double)r;
- *mb=(double)(*mb)/(double)r;
- *ml=(double)(*ml)/(double)r;
-
-
- r=max((double)*ma,(double)*mb);
- r=max((double)r,(double)*ml);
-
- *ma=(double)(r)/(double)*ma;
- *mb=(double)(r)/(double)*mb;
- *ml=(double)(r)/(double)*ml;
- }
- else
- {
- cerr << *ml << endl;
- cerr << *ma << endl;
- cerr << *mb << endl;
-
- double r=max((double)*ml,(double)*ma);
- r=max((double)r,(double)*mb);
- *ma=(double)(r)/(double)*ma;
- *mb=(double)(r)/(double)*mb;
- *ml=(double)(r)/(double)*ml;
- }
-}
-
-Buoy::Buoy(std::string name) : _it(_n), _s(_n, name, boost::bind(&Buoy::executeCB, this, _1), false), _actionName(name)
-{
- _sub = _it.subscribe(topics::CAMERA_FRONT_RAW_IMAGE, 1, &Buoy::imageCallBack, this);
- _pub = _it.advertise(topics::CAMERA_FRONT_BUOY_IMAGE, 1);
-
- ifstream _thresholdVal("threshold.th");
-
- if(_thresholdVal.is_open())
- {
- for(int i = 0; i < 3; i++)
- {
- _thresholdVal >> _lowerThreshRed1[i];
- }
-
- for(int i = 0; i < 3; i++)
- {
- _thresholdVal >> _upperThreshRed1[i];
- }
-
- for(int i = 0; i < 3; i++)
- {
- _thresholdVal >> _lowerThreshRed2[i];
- }
-
- for(int i = 0; i < 3; i++)
- {
- _thresholdVal >> _upperThreshRed2[i];
- }
-
- for(int i = 0; i < 3; i++)
- {
- _thresholdVal >> _lowerThreshGreen[i];
- }
-
- for(int i = 0; i < 3; i++)
- {
- _thresholdVal >> _upperThreshGreen[i];
- }
- }
- else
- {
- ROS_ERROR("Unable to open threshold file.");
- ros::shutdown();
- }
-
- _kernelDilateErode = getStructuringElement(MORPH_RECT, Size(3,3));
- _finalImage.encoding = "mono8";
- _s.start();
-}
-
-void Buoy::imageCallBack(const sensor_msgs::ImageConstPtr &_msg)
-{
- cv_bridge::CvImagePtr _imagePtr;
-
- try
- {
- _imagePtr = cv_bridge::toCvCopy(_msg, "bgr8");
- }
- catch (cv_bridge::Exception& e)
- {
- ROS_ERROR("Could not convert from '%s' to 'bgr8'.", _msg->encoding.c_str());
- }
-
- _image = _imagePtr->image;
-}
-
-void Buoy::executeCB(const actionmsg::buoyGoalConstPtr &_goal)
-{
- ros::Rate looprate(10);
- bool success = true;
-
- cout <<"<<<<<< _goal->order = >>>>> " << _goal-> order;
-
- switch(_goal->order)
- {
- case DETECT_BUOY:
- {
- bool _detected = false;
-
- while(ros::ok())
- {
- if (_s.isPreemptRequested() || !ros::ok())
- {
- ROS_INFO("%s: Preempted", _actionName.c_str());
- _s.setPreempted();
- success = false;
- break;
- }
-
- _detected = detectBuoy();
- _finalImage.image = _imageBW;
- _finalImagemsg = _finalImage.toImageMsg();
- _pub.publish(_finalImagemsg);
-
- if(_detected)
- {
- _result.sequence.push_back(BUOY_DETECTED);
- cout << "<<<<<<<<<< buoy has been detected.. >>>>>>>>>>>>>";
- break;
- }
-
- looprate.sleep();
- }
-
- break;
- }
-
- case ALIGN_BUOY:
- {
- while(ros::ok())
- {
- if (_s.isPreemptRequested() || !ros::ok())
- {
- ROS_INFO("%s: Preempted", _actionName.c_str());
- _s.setPreempted();
- success = false;
- break;
- }
-
- detectBuoy();
- getAllignment();
- _finalImage.image = _imageBW;
- _finalImagemsg = _finalImage.toImageMsg();
- _pub.publish(_finalImagemsg);
-
- if((_feedback.errorx < 10 && _feedback.errorx > -10) && (_feedback.errory > -10 && _feedback.errory < 10))
- {
- _result.sequence.push_back(BUOY_ALIGNED);
- cout << "<<<<<<<<<<<< BUOY_ALIGNED >>>>>>>>>>>>>>" << endl;
- break;
- }
-
- _s.publishFeedback(_feedback);
- looprate.sleep();
- }
-
- break;
- }
- }
-
- if(success)
- {
- _result.sequence.push_back(_goal->order);
- _s.setSucceeded(_result);
- }
-}
-
-
-bool Buoy::detectBuoy()
-{
- if(!_image.empty())
- {
- Mat org,_finalImg;
- org = _image;
- cout << "org image is being displayed; "<< endl;
-
- imshow("org" , _image);
-
- // imshow("maxRGB",b3.run(org,6,0));
- imshow("maxRGB",obj.run(org,1,0));
- imshow("max edge",obj1.run(org,1,0));
-
-
- waitKey(33);
- return 0;
- }
-
- return 0;
-}
-
-void Buoy::getAllignment()
-{
- for(int i = 0; i < _center.size(); i++)
- {
- _feedback.errorx = _image.cols/2 - _center[i].x;
- _feedback.errory = _image.rows/2 - _center[i].y;
- cout << "errorx and errory respectively : " ;
- cout<< _feedback.errorx << " and " << _feedback.errory << endl;
- }
-}
-
-Buoy::~Buoy()
-{
-}
-
-int main(int argc, char ** argv)
-{
- ros::init(argc, argv, "buoy_server");
- Buoy _buoyserver("buoy");
- ros::spin();
- return 0;
-}
diff --git a/vision_stack/task_buoy/src/oldbuoy.cpp b/vision_stack/task_buoy/src/oldbuoy.cpp
deleted file mode 100644
index 419e96b..0000000
--- a/vision_stack/task_buoy/src/oldbuoy.cpp
+++ /dev/null
@@ -1,755 +0,0 @@
-#include <ros/ros.h>
-#include <task_buoy/buoy_server.h>
-#define PI 3.1414
-
-#include <resources/topicHeader.h>
-
-
-class maxRGB
-{
-
-
-
-public:
-
- /** main function to call for maxRGB color correction **/
- Mat run(Mat,int p,int m);
-
-
- /** function coputes the illumination estimate max RGB algorithm.The function takes input image,minkowski norm and normalization method .
- * the output are ml,,ma,mb for each of the color channels
- **/
-
- void process(Mat src1,float *ml,float *ma,float *mb,int p,int m);
-
-
-};
-
-class max_edge
-{
-public:
- /** main function to call to perform max-edge color correction */
- Mat run(Mat,int p,int m);
-
- /** defines convolution type **/
- enum ConvolutionType
- {
- /* Return the full convolution, including border */
- CONVOLUTION_FULL,
-
- /* Return only the part that corresponds to the original image */
- CONVOLUTION_SAME,
-
- /* Return only the submatrix containing elements that were not influenced by the border */
- CONVOLUTION_VALID
- };
-
- /** method to perform 2D convolution */
- void conv2(const Mat &img, const Mat& kernel, ConvolutionType type, Mat& dest);
-
- /** function that computes illumination vector for max_edge algorithm**/
- void process(Mat src1,float *ml,float *ma,float *mb,int p,int m);
-};
-
-
-
-
-
-
-
-
-
-Buoy::Buoy(std::string name) : _it(_n), _s(_n, name, boost::bind(&Buoy::executeCB, this, _1), false), _actionName(name)
-{
- _sub = _it.subscribe(topics::CAMERA_FRONT_RAW_IMAGE, 1, &Buoy::imageCallBack, this);
- _pub = _it.advertise(topics::CAMERA_FRONT_BUOY_IMAGE, 1);
-
- ifstream _thresholdVal("threshold.th");
-
- if(_thresholdVal.is_open())
- {
- for(int i = 0; i < 3; i++)
- {
- _thresholdVal >> _lowerThreshRed1[i];
- }
-
- for(int i = 0; i < 3; i++)
- {
- _thresholdVal >> _upperThreshRed1[i];
- }
-
- for(int i = 0; i < 3; i++)
- {
- _thresholdVal >> _lowerThreshRed2[i];
- }
-
- for(int i = 0; i < 3; i++)
- {
- _thresholdVal >> _upperThreshRed2[i];
- }
-
- for(int i = 0; i < 3; i++)
- {
- _thresholdVal >> _lowerThreshGreen[i];
- }
-
- for(int i = 0; i < 3; i++)
- {
- _thresholdVal >> _upperThreshGreen[i];
- }
- }
- else
- {
- ROS_ERROR("Unable to open threshold file.");
- ros::shutdown();
- }
-
- _kernelDilateErode = getStructuringElement(MORPH_RECT, Size(3,3));
- _finalImage.encoding = "mono8";
- _s.start();
-}
-
-void Buoy::imageCallBack(const sensor_msgs::ImageConstPtr &_msg)
-{
- cv_bridge::CvImagePtr _imagePtr;
-
- try
- {
- _imagePtr = cv_bridge::toCvCopy(_msg, "bgr8");
- }
- catch (cv_bridge::Exception& e)
- {
- ROS_ERROR("Could not convert from '%s' to 'bgr8'.", _msg->encoding.c_str());
- }
-
- _image = _imagePtr->image;
-}
-
-void Buoy::executeCB(const actionmsg::buoyGoalConstPtr &_goal)
-{
- ros::Rate looprate(10);
- bool success = true;
-
- cout <<"<<<<<< _goal->order = >>>>> " << _goal-> order;
-
- switch(_goal->order)
- {
- case DETECT_BUOY:
- {
- bool _detected = false;
-
- while(ros::ok())
- {
- if (_s.isPreemptRequested() || !ros::ok())
- {
- ROS_INFO("%s: Preempted", _actionName.c_str());
- _s.setPreempted();
- success = false;
- break;
- }
-
- _detected = detectBuoy();
- _finalImage.image = _imageBW;
- _finalImagemsg = _finalImage.toImageMsg();
- _pub.publish(_finalImagemsg);
-
- if(_detected)
- {
- _result.sequence.push_back(BUOY_DETECTED);
- cout << "<<<<<<<<<< buoy has been detected.. >>>>>>>>>>>>>";
- break;
- }
-
- looprate.sleep();
- }
-
- break;
- }
-
- case ALIGN_BUOY:
- {
- while(ros::ok())
- {
- if (_s.isPreemptRequested() || !ros::ok())
- {
- ROS_INFO("%s: Preempted", _actionName.c_str());
- _s.setPreempted();
- success = false;
- break;
- }
-
- detectBuoy();
- getAllignment();
- _finalImage.image = _imageBW;
- _finalImagemsg = _finalImage.toImageMsg();
- _pub.publish(_finalImagemsg);
-
- if((_feedback.errorx < 10 && _feedback.errorx > -10) && (_feedback.errory > -10 && _feedback.errory < 10))
- {
- _result.sequence.push_back(BUOY_ALIGNED);
- cout << "<<<<<<<<<<<< BUOY_ALIGNED >>>>>>>>>>>>>>" << endl;
- break;
- }
-
- _s.publishFeedback(_feedback);
- looprate.sleep();
- }
-
- break;
- }
- }
-
- if(success)
- {
- _result.sequence.push_back(_goal->order);
- _s.setSucceeded(_result);
- }
-}
-
-
-
-//white balance
-
-Mat color_correction::maxRGB::run(Mat src1,int p,int m)
-{
-
- Mat dst;
-
- src1.copyTo(dst);
-
- cv::Mat_<cv::Vec3b>::const_iterator it= src1.begin<cv::Vec3b>();
- cv::Mat_<cv::Vec3b>::const_iterator itend= src1.end<cv::Vec3b>();
- cv::Mat_<cv::Vec3b>::iterator itout= dst.begin<cv::Vec3b>();
-
- float ma=0,mb=0,ml=0;
- process(src1,&ml,&ma,&mb,p,m);
-
- for ( ; it!= itend; ++it, ++itout)
- {
-
-
- cv::Vec3b v1=*it;
-
- float l=v1.val[0];
- float a=v1.val[1];
- float b=v1.val[2];
-
- if(m==0)
- {
- a=a*(ma);
- b= b*(mb);
- l= l*(ml);
- }
- else
- {
- //if(a<(float)95*255/100)
- a=a*(ma);
- //if(b<(float)95*255/100)
- b= b*(mb);
- // if(l<(float)95*255/100)
- l= l*(ml);
- }
-
- if(a>255)
- {
- a=255;
- }
-
- if(b>255)
- {
- b=255;
- }
-
- if(l>255)
- {
- l=255;
- }
-
-
- v1.val[0]=l;
-
- v1.val[1]=a;
-
- v1.val[2]=b;
-
- *itout=v1;
-
-
-
- }
-
- return dst;
-}
-
-
-
-void color_correction::maxRGB::process(Mat src1,float *ml,float *ma,float *mb,int p,int m)
-{
-
- Mat src;
-
- src1.convertTo(src,CV_32FC3,1.0,0);
-
- *ma=0;
- *mb=0;
- *ml=0;
-
- for(int i=0; i<src.rows; i++)
- {
-
- for(int j=0; j<src.cols; j++)
- {
- Vec3f v1=src.at<cv::Vec3f>(i,j);
- double lc=v1.val[0];
- double ac=v1.val[1];
- double bc=v1.val[2];
- *ml=max((double)*ml,(double)lc);
- *ma=max((double)*ma,(double)ac);
- *mb=max((double)*mb,bc);
-
- }
- }
-
-
-
-
-
- if(m==1)
- {
- double r=(*ma+*mb+*ml)/3;
- r=(double)sqrt((double)(*ma)*(*ma)+(double)(*mb)*(*mb)+(double)(*ml)*(*ml));
- *ma=(double)(*ma)/(double)r;
- *mb=(double)(*mb)/(double)r;
- *ml=(double)(*ml)/(double)r;
-
-
- r=max((double)*ma,(double)*mb);
- r=max((double)r,(double)*ml);
-
- *ma=(double)(r)/(double)*ma;
- *mb=(double)(r)/(double)*mb;
- *ml=(double)(r)/(double)*ml;
-
-
-
- }
- else
- {
-
-
- cerr << *ml << endl;
- cerr << *ma << endl;
- cerr << *mb << endl;
-
- double r=max((double)*ml,(double)*ma);
- r=max((double)r,(double)*mb);
- *ma=(double)(r)/(double)*ma;
- *mb=(double)(r)/(double)*mb;
- *ml=(double)(r)/(double)*ml;
-
-
- }
-
-
-}
-
-
-
-Mat color_correction::max_edge::run( Mat src1,int p,int m)
-{
-
- Mat dst;
-
- src1.copyTo(dst);
-
- cv::Mat_<cv::Vec3b>::const_iterator it= src1.begin<cv::Vec3b>();
- cv::Mat_<cv::Vec3b>::const_iterator itend= src1.end<cv::Vec3b>();
- cv::Mat_<cv::Vec3b>::iterator itout= dst.begin<cv::Vec3b>();
-
- float ma=0,mb=0,ml=0;
- process(src1,&ml,&ma,&mb,p,m);
-
- for ( ; it!= itend; ++it, ++itout)
- {
-
-
- cv::Vec3b v1=*it;
-
- float l=v1.val[0];
- float a=v1.val[1];
- float b=v1.val[2];
-
- if(m==0)
- {
- a=a*(ma);
- b= b*(mb);
- l= l*(ml);
- }
- else
- {
- //if(a<(float)95*255/100)
- a=a*(ma);
- //if(b<(float)95*255/100)
- b= b*(mb);
- // if(l<(float)95*255/100)
- l= l*(ml);
- }
-
- if(a>255)
- {
- a=255;
- }
-
- if(b>255)
- {
- b=255;
- }
-
- if(l>255)
- {
- l=255;
- }
-
-
- v1.val[0]=l;
-
- v1.val[1]=a;
-
- v1.val[2]=b;
-
- *itout=v1;
-
-
-
- }
-
- return dst;
-}
-
-
-
-void color_correction::max_edge::conv2(const Mat &img, const Mat& kernel, ConvolutionType type, Mat& dest)
-{
- Mat source = img;
-
- if(CONVOLUTION_FULL == type)
- {
- source = Mat();
- const int additionalRows = kernel.rows-1, additionalCols = kernel.cols-1;
- copyMakeBorder(img, source, (additionalRows+1)/2, additionalRows/2, (additionalCols+1)/2, additionalCols/2, BORDER_CONSTANT, Scalar(0));
- }
-
- Point anchor(kernel.cols - kernel.cols/2 - 1, kernel.rows - kernel.rows/2 - 1);
- int borderMode = BORDER_CONSTANT;
- flip(kernel,kernel,-1);
- filter2D(source, dest, img.depth(),kernel, anchor, 0, borderMode);
-
- if(CONVOLUTION_VALID == type)
- {
- dest = dest.colRange((kernel.cols-1)/2, dest.cols - kernel.cols/2)
- .rowRange((kernel.rows-1)/2, dest.rows - kernel.rows/2);
- }
-}
-
-
-void color_correction::max_edge::process(Mat src1,float *ml,float *ma,float *mb,int p,int m)
-{
-
- *ma=0;
- *mb=0;
- *ml=0;
-
-
- Mat src;
- vector<Mat> bgr_planes;
- Mat src2;
- //src1.convertTo(src2,CV_32FC3,1.0/255.0,0);
- src1.convertTo(src,CV_32FC3,1.0,0);
- split( src, bgr_planes );
-
- Mat kernel=getGaussianKernel(100,6,CV_32F);
-
-
- Mat grad_x, grad_y,abs_angle_x;
- Mat grad1,grad2,grad3;
- Mat r1,g,b;
-
- cerr << bgr_planes[0].depth() << ":" << CV_32FC1<< "dddddd" << endl;
-
- conv2( bgr_planes[0], kernel, CONVOLUTION_FULL,r1);
- conv2( bgr_planes[1], kernel, CONVOLUTION_FULL,g);
- conv2( bgr_planes[2], kernel, CONVOLUTION_FULL,b);
-
- //cv::imshow("dddd",b);
- // 1 2 1 1 0 -1
- // 0 0 0 2 0 -2
- // -1 -2 -1 1 0 -1
- double dx[]= {1,2,1,0,0,0,-1,-2,-1};
- double dy[]= {1,0,-1,2,0,-2,1,0,-1};
-
- Mat kx=Mat(3,3,CV_32FC1,dx);
- Mat ky=Mat(3,3,CV_32FC1,dy);
-
-
- grad_x.setTo(cv::Scalar::all(0));
- grad_y.setTo(cv::Scalar::all(0));
-
- conv2(r1, kx, CONVOLUTION_FULL,grad_x);
- conv2(r1, ky, CONVOLUTION_FULL,grad_y);
-
-
- cartToPolar(grad_x,grad_y,bgr_planes[0],abs_angle_x,false);
- grad_x.setTo(cv::Scalar::all(0));
- grad_y.setTo(cv::Scalar::all(0));
-
- conv2(g, kx, CONVOLUTION_FULL,grad_x);
- conv2(g, ky, CONVOLUTION_FULL,grad_y);
-
- cartToPolar(grad_x,grad_y,bgr_planes[1],abs_angle_x,false);
- grad_x.setTo(cv::Scalar::all(0));
- grad_y.setTo(cv::Scalar::all(0));
-
- conv2(b, kx, CONVOLUTION_FULL,grad_x);
- conv2(b, ky, CONVOLUTION_FULL,grad_y);
-
- cartToPolar(grad_x,grad_y,bgr_planes[2],abs_angle_x,false);
-
-
-
-
-
-
-
- cv::merge(bgr_planes,src);
-
- for(int i=0; i<src.rows; i++)
- {
-
- for(int j=0; j<src.cols; j++)
- {
- Vec3f v1=src.at<cv::Vec3f>(i,j);
- double lc=v1.val[0];
- double ac=v1.val[1];
- double bc=v1.val[2];
-// *ma=*ma+ac;
-// *mb=*mb+bc;
-// *ml=*ml+lc;
- //cerr << lc << ":" ;
- *ml=max((double)*ml,(double)lc);
- *ma=max((double)*ma,(double)ac);
- *mb=max((double)*mb,(double)bc);
- }
- }
-
-
-// *ma=pow((double)*ma,(double)1/p);
-// *mb=pow((double)*mb,(double)1/p);
-// *ml=pow((double)*ml,(double)1/p);
-
-
- double r=0;
-
- if(m==0)
- {
- r=(*ma+*mb+*ml)/3;
-
- *ma=r/(*ma);
- *mb=r/(*mb);
- *ml=r/(*ml);
- cerr << *ml << endl;
- cerr << *ma << endl;
- cerr << *mb << endl;
-
- }
-
- if(m==1)
- {
- r=(*ma+*mb+*ml)/3;
- r=max((double)*ma,(double)*mb);
- r=max((double)r,(double)*ml);
-
- *ma=r/(*ma);
- *mb=r/(*mb);
- *ml=r/(*ml);
- cerr << *ml << endl;
- cerr << *ma << endl;
- cerr << *mb << endl;
-
- }
-
- if(m==2)
- {
- r=sqrt((*ma)*(*ma)+(*mb)*(*mb)+(*ml)*(*ml));
- *ma=(double)(*ma)/(double)r;
- *mb=(double)(*mb)/(double)r;
- *ml=(double)(*ml)/(double)r;
-
- cerr << *ml << endl;
- cerr << *ma << endl;
- cerr << *mb << endl;
-
- r=max((double)*ma,(double)*mb);
- r=max((double)r,(double)*ml);
-
- *ma=sqrt((double)r/(double)(*ma));
- *mb=sqrt((double)r/(double)(*mb));
- *ml=sqrt((double)r/(double)(*ml));
- }
-
-
-
-
-}
-
-
-
-
-
-
-
-
-
-bool Buoy::detectBuoy()
-{
- if(!_image.empty())
- {
- Mat org,_finalImg;
- org = _image;
- cout << "org image is being displayed; "<< endl;
-
- imshow("org" , _image);
-
- imshow("maxRGB",b3.run(org,6,0));
- imshow("max edge",b4.run(org,1,0));
-
-
-
- waitKey(33);
- return 0;
- }
-
- return 0;
- // cvtColor(_image, _imageHSV, CV_BGR2HSV);
-
- // inRange(_imageHSV,_lowerThreshRed1,_upperThreshRed1, _imageBW);
- // inRange(_imageHSV,_lowerThreshRed2,_upperThreshRed2, _imageBWRed);
- // add(_imageBW, _imageBWRed, _imageBW);
-
- // inRange(_imageHSV,_lowerThreshGreen,_upperThreshGreen, _imageBWGreen);
- // add(_imageBW, _imageBWGreen, _imageBW);
- // imshow("_imageBW", _imageBW);
- // waitKey(33);
-
- // medianBlur(_imageBW, _imageBW, 3);
- // erode(_imageBW, _imageBW, _kernelDilateErode);
- // CBlobResult _blobs,_blobsClutter;
- // CBlob * _currentBlob;
- // IplImage _imageBWipl = _imageBW;
-
- // _blobs = CBlobResult(&_imageBWipl, NULL, 0);
- // _blobs.Filter(_blobs, B_INCLUDE, CBlobGetArea(), B_INSIDE, 50, 1000);
-
- // _imageBW = Scalar(0, 0, 0);
- // for(int i = 0; i < _blobs.GetNumBlobs(); i++)
- // {
- // _currentBlob = _blobs.GetBlob(i);
- // _currentBlob->FillBlob(&_imageBWipl, Scalar(255));
- // }
-
- // Mat _imageBW2 = _imageBW.clone();
- // Mat src;
- // vector<Mat> channels;
- // channels.push_back(_imageBW);
- // channels.push_back(_imageBW);
- // channels.push_back(_imageBW);
- // merge( channels, src);
-
- // _contours.clear();
- // medianBlur(_imageBW2, _imageBW2, 5);
- // findContours(_imageBW2, _contours, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
-
- // Point2f _centerBuff;
- // float _radiusBuff;
- // vector<Point> _contoursPolyBuff;
- // _center.clear();
- // _radius.clear();
- // _contoursPoly.clear();
-
- // _imageBW = Scalar(0, 0, 0);
-
- // for(int i=0; i < _contours.size(); i++)
- // {
- // if(contourArea(_contours[i])>50)
- // {
- // approxPolyDP(_contours[i],_contoursPolyBuff,3,true);
- // minEnclosingCircle((Mat)_contoursPolyBuff,_centerBuff,_radiusBuff);
- // circle(_imageBW,_centerBuff,_radiusBuff,Scalar(255), -1);
- // _center.push_back(_centerBuff);
- // _radius.push_back(_radiusBuff);
- // _contoursPoly.push_back(_contoursPolyBuff);
- // }
- // }
-
-
- // Mat src_gray;
- // cvtColor( src, src_gray, CV_BGR2GRAY );
- // src = Scalar(0, 0, 0);
-
- // GaussianBlur( src_gray, src_gray, Size(9, 9), 2, 2 );
- // imshow("src_gray", src_gray);
-
- // vector<Vec3f> circles;
-
- // /// Apply the Hough Transform to find the circles
- // HoughCircles( src_gray, circles, CV_HOUGH_GRADIENT, 1, src_gray.rows/16, 150, 25, 0, 0 );
-
- // /// Draw the circles detected
- // if(circles.size() == 0)
- // cout<<"NOTHING CIRCULAR" << endl;
-
- // for( size_t i = 0; i < circles.size(); i++ )
- // {
- // Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
- // int radius = cvRound(circles[i][2]);
- // // circle center
- // circle( src, center, 3, Scalar(0, 255, 0), 3, 8, 0 );
- // // circle outline
- // circle( src, center, radius, Scalar(0, 0, 255), 1, 8, 0 );
- // }
-
- // imshow("src", src);
-
- // if(_center.size() > 0)
- // return true;
- // else
- // return false;
- // }
- // else
- // return false;
-}
-
-void Buoy::getAllignment()
-{
- for(int i = 0; i < _center.size(); i++)
- {
- _feedback.errorx = _image.cols/2 - _center[i].x;
- _feedback.errory = _image.rows/2 - _center[i].y;
- cout << "errorx and errory respectively : " ;
- cout<< _feedback.errorx << " and " << _feedback.errory << endl;
- }
-}
-
-
-
-Buoy::~Buoy()
-{
-
-}
-
-int main(int argc, char ** argv)
-{
- ros::init(argc, argv, "buoy_server");
- Buoy _buoyserver("buoy");
- ros::spin();
- return 0;
-}
diff --git a/vision_stack/task_buoy/threshold.th b/vision_stack/task_buoy/threshold.th
old mode 100755
new mode 100644
index 1747997..a4b5e88
--- a/vision_stack/task_buoy/threshold.th
+++ b/vision_stack/task_buoy/threshold.th
@@ -1,12 +1,18 @@
-30
0
0
-60
+0
+10
+255
+255
+170
+0
+0
+180
255
255
-100
+35
0
0
-160
+70
255
255
\ No newline at end of file
diff --git a/vision_stack/task_buoy/threshold2.th b/vision_stack/task_buoy/threshold2.th
deleted file mode 100644
index a4b5e88..0000000
--- a/vision_stack/task_buoy/threshold2.th
+++ /dev/null
@@ -1,18 +0,0 @@
-0
-0
-0
-10
-255
-255
-170
-0
-0
-180
-255
-255
-35
-0
-0
-70
-255
-255
\ No newline at end of file
diff --git a/vision_stack/task_marker/src/marker_client.cpp b/vision_stack/task_marker/src/marker_client.cpp
index 5a129a3..8629bdb 100644
--- a/vision_stack/task_marker/src/marker_client.cpp
+++ b/vision_stack/task_marker/src/marker_client.cpp
@@ -5,9 +5,6 @@
#include <task_marker/resultheader.h>
#include <task_marker/taskheader.h>
-
-using namespace std;
-
typedef actionlib::SimpleActionClient<ip_msgs::markerAction> Client;
void done_cb(const actionlib::SimpleClientGoalState& state,const ip_msgs::markerResultConstPtr& result)
@@ -35,7 +32,6 @@ int main(int argc, char ** argv)
ip_msgs::markerGoal _goal;
_goal.order = DETECT_MARKER;
- cout << "detetct marker goal sent" << endl;
_client.sendGoal(_goal,&done_cb,
&activeCb ,
&feedback_cb);
@@ -55,7 +51,6 @@ int main(int argc, char ** argv)
}
_goal.order = ALIGN_MARKER;
- cout << "align marker goal sent" << endl;
_client.sendGoal(_goal);
_actionStatus = _client.waitForResult(ros::Duration(300.0));
diff --git a/vision_stack/task_marker/src/marker_server.cpp b/vision_stack/task_marker/src/marker_server.cpp
index 8cb8905..e41f116 100644
--- a/vision_stack/task_marker/src/marker_server.cpp
+++ b/vision_stack/task_marker/src/marker_server.cpp
@@ -5,12 +5,7 @@
Marker::Marker(std::string name, std::string _threshold_filepath) : _it(_n), _s(_n, name, boost::bind(&Marker::executeCB, this, _1), false), _actionName(name)
{
- // camera is changed to frontcam for testing purpose
- // when bottomcam starts working it has to be changed to bottomcam
-
- _sub = _it.subscribe(topics::CAMERA_FRONT_RAW_IMAGE, 1, &Marker::imageCallBack, this);
- // _sub = _it.subscribe(topics::CAMERA_BOTTOM_RAW_IMAGE, 1, &Marker::imageCallBack, this);
-
+ _sub = _it.subscribe(topic::CAMERA_BOTTOM_RAW_IMAGE, 1, &Marker::imageCallBack, this);
// _pub = _it.advertise("/kraken/bottomcam/marker_image", 1);
_pub = _it.advertise(topics::CAMERA_BOTTOM_MARKER_IMAGE, 1);
marker_detect_status=false;
@@ -138,13 +133,6 @@ void Marker::executeCB(const ip_msgs::markerGoalConstPtr &_goal)
void Marker::detectMarker()
{
- if (I.empty())
- {
- cout << "image not loaded..";
- marker_detect_status = false;
- return;
- }
-
cvtColor(I, I_hsv, CV_BGR2HSV);
inRange(I_hsv, _lowerThresh, _upperThresh, I_bw);
medianBlur(I_bw, I_bw, 3);
@@ -184,25 +172,16 @@ void Marker::detectMarker()
if(contourArea(_contours[i])>400)
{
_minRect.push_back(minAreaRect( Mat(_contours[i])));
- drawContours(I,_contours,i,Scalar(0,255,255),4);
marker_detect_status=true;
- cout << "marker detected.. " << endl;
}
}
-
- imshow("I",I);
-
- if (cvWaitKey(33) == 27)
- {
- return;
- }
}
void Marker::getAllignment()
{
for( int i = 0; i< _minRect.size(); i++ )
{
- Scalar _color = Scalar( Scalar(255,0,0) );
+ Scalar _color = Scalar( Scalar(255,0,0) );
Point2f _rectPoints[4];
_minRect[i].points( _rectPoints );
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment