ActionFeedback.java
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2011, 2012
00004  *
00005  * School of Engineering, Cardiff University, UK
00006  *
00007  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00008  *
00009  * Project name: srs EU FP7 (www.srs-project.eu)
00010  * ROS stack name: srs
00011  * ROS package name: srs_knowledge
00012  * Description: 
00013  *                                                              
00014  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00015  *
00016  * @author Ze Ji, email: jiz1@cf.ac.uk
00017  *
00018  * Date of creation: Oct 2011:
00019  * ToDo: 
00020  *
00021  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00022  *
00023  * Redistribution and use in source and binary forms, with or without
00024  * modification, are permitted provided that the following conditions are met:
00025  *
00026  *       * Redistributions of source code must retain the above copyright
00027  *         notice, this list of conditions and the following disclaimer.
00028  *       * Redistributions in binary form must reproduce the above copyright
00029  *         notice, this list of conditions and the following disclaimer in the
00030  *         documentation and/or other materials provided with the distribution.
00031  *       * Neither the name of the school of Engineering, Cardiff University nor 
00032  *         the names of its contributors may be used to endorse or promote products 
00033  *         derived from this software without specific prior written permission.
00034  *
00035  * This program is free software: you can redistribute it and/or modify
00036  * it under the terms of the GNU Lesser General Public License LGPL as 
00037  * published by the Free Software Foundation, either version 3 of the 
00038  * License, or (at your option) any later version.
00039  * 
00040  * This program is distributed in the hope that it will be useful,
00041  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00042  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00043  * GNU Lesser General Public License LGPL for more details.
00044  * 
00045  * You should have received a copy of the GNU Lesser General Public 
00046  * License LGPL along with this program. 
00047  * If not, see <http://www.gnu.org/licenses/>.
00048  *
00049  ****************************************************************/
00050 
00051 package org.srs.srs_knowledge.task;
00052 
00053 import java.io.*;
00054 import java.util.ArrayList;
00055 import ros.pkg.geometry_msgs.msg.Pose2D;
00056 import ros.pkg.geometry_msgs.msg.Pose;
00057 import org.srs.srs_knowledge.knowledge_engine.*;
00058 import org.srs.srs_knowledge.task.Task;
00059 
00060 import org.json.simple.JSONArray;
00061 import org.json.simple.JSONObject;
00062 import org.json.simple.JSONValue;
00063 import org.json.simple.parser.ParseException;
00064 import org.json.simple.parser.JSONParser;
00065 
00066 public class ActionFeedback {
00067     public ActionFeedback(String jsonGenericFeedback) throws ParseException {
00068         this.jsonFeedback = jsonGenericFeedback;
00069         JSONParser parser = new JSONParser();
00070         Object obj = parser.parse(this.jsonFeedback);
00071         this.jsonFBObject = (JSONObject)obj;
00072 
00073         System.out.println("Constructor: using json feedback.");
00074     }
00075 
00076     public Pose getDetectedObjectPose() {
00077         Pose pos = new Pose();
00078 
00079         try {
00080             JSONObject fb = (JSONObject)this.jsonFBObject.get("feedback");
00081             String actionName = (String)fb.get("action");
00082             if(!actionName.equals("detect")) {
00083                 System.out.println("Action type is not 'detect'... Format error");
00084                 return null;        
00085             }
00086             JSONObject detObj = (JSONObject)fb.get("object");
00087             
00088             String objectName = (String)detObj.get("object_type");
00089             System.out.println("Detected object: " + objectName);
00090             JSONObject jsonPos = (JSONObject)fb.get("pose");
00091             pos.position.x = ((Number)(jsonPos.get("x"))).doubleValue();
00092             pos.position.y = ((Number)(jsonPos.get("y"))).doubleValue();
00093             pos.position.z = ((Number)(jsonPos.get("z"))).doubleValue();
00094             pos.orientation.x = ((Number)(jsonPos.get("rotx"))).doubleValue();
00095             pos.orientation.y = ((Number)(jsonPos.get("roty"))).doubleValue();
00096             pos.orientation.z = ((Number)(jsonPos.get("rotz"))).doubleValue();
00097             pos.orientation.w = ((Number)(jsonPos.get("rotw"))).doubleValue();    
00098             System.out.println(this.jsonFeedback);
00099             //System.out.println("json decode pose as: " + pos);
00100         }
00101         catch(Exception e) {
00102             System.out.println(e.toString());
00103             return null;
00104         }
00105 
00106         return pos;
00107     }
00108 
00109     private String jsonFeedback = "";
00110     private JSONObject jsonFBObject = new JSONObject();
00111 }


srs_knowledge
Author(s): Ze Ji
autogenerated on Mon Oct 6 2014 08:27:32