init commit no box data

CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mie443_contest2)
+
+set(CMAKE_CXX_FLAGS "-std=c++11")
+
+find_package(OpenCV 3 REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+    actionlib
+    move_base_msgs
+    roscpp
+    cv_bridge
+    image_transport
+    gazebo_ros
+)
+find_package(orocos_kdl REQUIRED)
+
+catkin_package()
+
+include_directories(include  
+    ${catkin_INCLUDE_DIRS}  
+    ${OpenCV_INCLUDE_DIRS})
+
+# Contest 2 Executable
+add_executable(contest2 
+    src/contest2.cpp
+    src/boxes.cpp
+    src/navigation.cpp
+    src/robot_pose.cpp
+    src/imagePipeline.cpp)
+target_link_libraries(contest2 
+    ${catkin_LIBRARIES}  
+    ${OpenCV_LIBRARIES}  
+    orocos-kdl)
+
+# Webcam Publisher Executable
+add_executable(webcam_publisher 
+    src/webcam_publisher.cpp)
+target_link_libraries(webcam_publisher  
+    ${catkin_LIBRARIES}  
+    ${OpenCV_LIBRARIES})

include/boxes.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include <opencv2/highgui/highgui.hpp>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_broadcaster.h>
+#include <vector>
+
+class Boxes {
+	public:
+		std::vector<cv::Mat> templates;
+		std::vector<std::vector<float>> coords;
+	public:
+		bool load_coords();
+		bool load_templates();
+};

include/imagePipeline.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <image_transport/image_transport.h>
+#include <std_msgs/String.h>
+#include <opencv2/core.hpp>
+#include <cv.h>
+#include <cv_bridge/cv_bridge.h>
+#include <boxes.h>
+
+class ImagePipeline {
+    private:
+        cv::Mat img;
+        bool isValid;
+        image_transport::Subscriber sub;
+    public:
+        ImagePipeline(ros::NodeHandle& n);
+        void imageCallback(const sensor_msgs::ImageConstPtr& msg);
+        int getTemplateID(Boxes& boxes);
+};

include/navigation.h

@@ -0,0 +1,6 @@
+#pragma once
+
+class Navigation {
+	public:
+		static bool moveToGoal(float xGoal, float yGoal, float phiGoal);
+};

include/robot_pose.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+
+class RobotPose {
+	public:
+		float x;
+		float y;
+		float phi;
+	public:
+		RobotPose(float x, float y, float phi);
+		void poseCallback(const geometry_msgs::PoseWithCovarianceStamped& msg);
+};

launch/turtlebot_world.launch

@@ -0,0 +1,38 @@
+<launch>
+  <arg name="gui"        default="true"/>
+  <arg name="world"      default="1"/>
+  <arg name="world_file" default="$(find mie443_contest2)/worlds/world_$(arg world).world"/>
+
+  <arg name="base"       value="$(optenv TURTLEBOT_BASE kobuki)"/> <!-- create, roomba -->
+  <arg name="battery"    value="$(optenv TURTLEBOT_BATTERY /proc/acpi/battery/BAT0)"/>  <!-- /proc/acpi/battery/BAT0 -->
+  <arg name="stacks"     value="$(optenv TURTLEBOT_STACKS hexagons)"/>  <!-- circles, hexagons -->
+  <arg name="3d_sensor"  value="$(optenv TURTLEBOT_3D_SENSOR kinect)"/>  <!-- kinect, asus_xtion_pro -->
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="use_sim_time" value="true"/>
+    <arg name="debug" value="false"/>
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="world_name" value="$(arg world_file)"/>
+  </include>
+
+  <include file="$(find turtlebot_gazebo)/launch/includes/$(arg base).launch.xml">
+    <arg name="base" value="$(arg base)"/>
+    <arg name="stacks" value="$(arg stacks)"/>
+    <arg name="3d_sensor" value="$(arg 3d_sensor)"/>
+  </include>
+
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+    <param name="publish_frequency" type="double" value="30.0" />
+  </node>
+
+  <!-- Fake laser -->
+  <node pkg="nodelet" type="nodelet" name="laserscan_nodelet_manager" args="manager"/>
+  <node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan"
+        args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet laserscan_nodelet_manager">
+    <param name="scan_height" value="10"/>
+    <param name="output_frame_id" value="/camera_depth_frame"/>
+    <param name="range_min" value="0.45"/>
+    <remap from="image" to="/camera/depth/image_raw"/>
+    <remap from="scan" to="/scan"/>
+  </node>
+</launch>

maps/map_practice.yaml

@@ -0,0 +1,7 @@
+image: map_practice.pgm
+resolution: 0.050000
+origin: [-12.200000, -12.200000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

package.xml

@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>mie443_contest2</name>
+  <version>0.0.0</version>
+  <description>second Lab for mie433</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">turtlebot</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/simple_navigation_goals</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>actionlib</depend>
+  <depend>move_base_msgs</depend>
+  <depend>roscpp</depend>
+  <depend>gazebo_ros</depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+    <gazebo_ros gazebo_model_path="${prefix}/worlds/models"/> 
+  </export>
+</package>

src/boxes.cpp

@@ -0,0 +1,83 @@
+#include <ros/package.h>
+#include <boxes.h>
+#include <tf/transform_datatypes.h>
+
+bool Boxes::load_coords() {
+    std::string filePath = ros::package::getPath("mie443_contest2") +
+                            std::string("/boxes_database/coords.xml");
+    cv::FileStorage fs(filePath, cv::FileStorage::READ);
+    if(fs.isOpened()) {
+        cv::FileNode node;
+        cv::FileNodeIterator it, end;
+        std::vector<float> coordVec;
+        std::string coords_xml[10] = {"coordinate1", "coordinate2", "coordinate3", "coordinate4",
+                                    "coordinate5", "coordinate6", "coordinate7", "coordinate8",
+                                    "coordinate9", "coordinate10"};
+        for(int i = 0; i < 10; ++i) {
+            node = fs[coords_xml[i]];
+            if(node.type() != cv::FileNode::SEQ) {
+                std::cout << "XML ERROR: Data in " << coords_xml[i]
+                            << " is improperly formatted - check input.xml" << std::endl;
+            } else {
+                it = node.begin();
+                end = node.end();
+                coordVec = std::vector<float>();
+                for(int j = 0; it != end; ++it, ++j) {
+                    coordVec.push_back((float)*it);
+                }
+                tf::Quaternion q(coordVec[3], coordVec[4], coordVec[5], coordVec[6]);
+                tf::Matrix3x3 m(q);
+                double roll, pitch, yaw;
+                m.getRPY(roll, pitch, yaw);
+                auto boxCoords = std::vector<float>();
+                boxCoords.push_back(coordVec[0]);
+                boxCoords.push_back(coordVec[1]);
+                boxCoords.push_back(yaw);
+                if(coordVec.size() == 7) {
+
+                    coords.push_back(boxCoords);
+                } else {
+                    std::cout << "XML ERROR: Data in " << coords_xml[i]
+                                << " is improperly formatted - check input.xml" << std::endl;
+                }
+            }
+        }
+        if(coords.size() == 0) {
+            std::cout << "XML ERROR: Coordinate data is improperly formatted - check input.xml"
+                        << std::endl;
+            return false;
+        }
+    } else {
+        std::cout << "Could not open XML - check FilePath in " << filePath << std::endl;
+        return false;
+    }
+    return true;
+}
+
+bool Boxes::load_templates() {
+    std::string filePath = ros::package::getPath("mie443_contest2") +
+                            std::string("/boxes_database/templates.xml");
+    cv::FileStorage fs(filePath, cv::FileStorage::READ);
+    if(fs.isOpened()) {
+        cv::FileNode node = fs["templates"];;
+        cv::FileNodeIterator it, end;
+        if(!(node.type() == cv::FileNode::SEQ || node.type() == cv::FileNode::STRING)) {
+            std::cout << "XML ERROR: Image data is improperly formatted in " << filePath
+                        << std::endl;
+            return false;
+        }
+        it = node.begin();
+        end = node.end();
+        std::string imagepath;
+        for(; it != end; ++it){
+            imagepath = ros::package::getPath("mie443_contest2") +
+                        std::string("/boxes_database/") +
+                        std::string(*it);
+            templates.push_back(cv::imread(imagepath, CV_LOAD_IMAGE_GRAYSCALE));
+        }
+    } else {
+        std::cout << "XML ERROR: Could not open " << filePath << std::endl;
+        return false;
+    }
+    return true;
+}

src/contest2.cpp

@@ -0,0 +1,36 @@
+#include <boxes.h>
+#include <navigation.h>
+#include <robot_pose.h>
+#include <imagePipeline.h>
+
+int main(int argc, char** argv) {
+    // Setup ROS.
+    ros::init(argc, argv, "contest2");
+    ros::NodeHandle n;
+    // Robot pose object + subscriber.
+    RobotPose robotPose(0,0,0);
+    ros::Subscriber amclSub = n.subscribe("/amcl_pose", 1, &RobotPose::poseCallback, &robotPose);
+    // Initialize box coordinates and templates
+    Boxes boxes; 
+    if(!boxes.load_coords() || !boxes.load_templates()) {
+        std::cout << "ERROR: could not load coords or templates" << std::endl;
+        return -1;
+    }
+    for(int i = 0; i < boxes.coords.size(); ++i) {
+        std::cout << "Box coordinates: " << std::endl;
+        std::cout << i << " x: " << boxes.coords[i][0] << " y: " << boxes.coords[i][1] << " z: " 
+                  << boxes.coords[i][2] << std::endl;
+    }
+    // Initialize image objectand subscriber.
+    ImagePipeline imagePipeline(n);
+    // Execute strategy.
+    while(ros::ok()) {
+        ros::spinOnce();
+        /***YOUR CODE HERE***/
+        // Use: boxes.coords
+        // Use: robotPose.x, robotPose.y, robotPose.phi
+        imagePipeline.getTemplateID(boxes);
+        ros::Duration(0.01).sleep();
+    }
+    return 0;
+}

src/imagePipeline.cpp

@@ -0,0 +1,42 @@
+#include <imagePipeline.h>
+
+#define IMAGE_TYPE sensor_msgs::image_encodings::BGR8
+#define IMAGE_TOPIC "camera/rgb/image_raw" // kinect:"camera/rgb/image_raw" webcam:"camera/image"
+
+ImagePipeline::ImagePipeline(ros::NodeHandle& n) {
+    image_transport::ImageTransport it(n);
+    sub = it.subscribe(IMAGE_TOPIC, 1, &ImagePipeline::imageCallback, this);
+    isValid = false;
+}
+
+void ImagePipeline::imageCallback(const sensor_msgs::ImageConstPtr& msg) {
+    try {
+        if(isValid) {
+            img.release();
+        }
+        img = (cv_bridge::toCvShare(msg, IMAGE_TYPE)->image).clone();
+        isValid = true;
+    } catch (cv_bridge::Exception& e) {
+        std::cout << "ERROR: Could not convert from " << msg->encoding.c_str()
+                  << " to " << IMAGE_TYPE.c_str() << "!" << std::endl;
+        isValid = false;
+    }    
+}
+
+int ImagePipeline::getTemplateID(Boxes& boxes) {
+    int template_id = -1;
+    if(!isValid) {
+        std::cout << "ERROR: INVALID IMAGE!" << std::endl;
+    } else if(img.empty() || img.rows <= 0 || img.cols <= 0) {
+        std::cout << "ERROR: VALID IMAGE, BUT STILL A PROBLEM EXISTS!" << std::endl;
+        std::cout << "img.empty():" << img.empty() << std::endl;
+        std::cout << "img.rows:" << img.rows << std::endl;
+        std::cout << "img.cols:" << img.cols << std::endl;
+    } else {
+        /***YOUR CODE HERE***/
+        // Use: boxes.templates
+        cv::imshow("view", img);
+        cv::waitKey(10);
+    }  
+    return template_id;
+}

src/navigation.cpp

@@ -0,0 +1,35 @@
+#include <navigation.h>
+#include <actionlib/client/simple_action_client.h>
+#include <move_base_msgs/MoveBaseAction.h>
+#include <tf/transform_datatypes.h>
+
+bool Navigation::moveToGoal(float xGoal, float yGoal, float phiGoal){
+	// Set up and wait for actionClient.
+    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base", true);
+    while(!ac.waitForServer(ros::Duration(5.0))){
+        ROS_INFO("Waiting for the move_base action server to come up");
+    }
+	// Set goal.
+    geometry_msgs::Quaternion phi = tf::createQuaternionMsgFromYaw(phiGoal);
+    move_base_msgs::MoveBaseGoal goal;
+    goal.target_pose.header.frame_id = "map";
+    goal.target_pose.header.stamp = ros::Time::now();
+    goal.target_pose.pose.position.x =  xGoal;
+    goal.target_pose.pose.position.y =  yGoal;
+    goal.target_pose.pose.position.z =  0.0;
+    goal.target_pose.pose.orientation.x = 0.0;
+    goal.target_pose.pose.orientation.y = 0.0;
+    goal.target_pose.pose.orientation.z = phi.z;
+    goal.target_pose.pose.orientation.w = phi.w;
+    ROS_INFO("Sending goal location ...");
+	// Send goal and wait for response.
+    ac.sendGoal(goal);
+    ac.waitForResult();
+    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+        ROS_INFO("You have reached the destination");
+        return true;
+    } else {
+        ROS_INFO("The robot failed to reach the destination");
+        return false;
+    }
+}