robot_viz

base_placement_plugin/CMakeLists.txt

@@ -15,6 +15,9 @@ find_package(catkin REQUIRED COMPONENTS
   tf_conversions
   map_creator
   pcl_ros
+  geometry_msgs
+  moveit_ros_planning_interface
+  moveit_core
 )
 
 find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED)
@@ -24,12 +27,18 @@ include(${QT_USE_FILE})
 
 add_definitions(-DQT_NO_KEYWORDS)
 
+
+add_library(create_marker  src/create_marker.cpp)
+target_link_libraries(create_marker  ${catkin_LIBRARIES} )
+
 set(base_placement_plugin_SRCS
   src/add_way_point.cpp
   src/point_tree_item.cpp
   src/point_tree_model.cpp
   src/widgets/base_placement_widget.cpp
   src/place_base.cpp
+  src/add_robot_base.cpp
+
 
   ${MOC_FILES}
 )
@@ -40,6 +49,8 @@ set(base_placement_plugin_HDRS
   include/base_placement_plugin/point_tree_model.h
   include/base_placement_plugin/widgets/base_placement_widget.h
   include/base_placement_plugin/place_base.h
+  include/base_placement_plugin/add_robot_base.h
+
 
 )
 
@@ -66,14 +77,16 @@ catkin_package(
   map_creator
 )
 
-qt4_wrap_cpp(base_placement_plugin_MOCS ${base_placement_plugin_HDRS})
-qt4_wrap_ui(base_placement_plugin_UIS_H ${base_placement_plugin_UIS})
+qt4_wrap_cpp(base_placement_plugin_MOCS ${base_placement_plugin_HDRS} )
+qt4_wrap_ui(base_placement_plugin_UIS_H ${base_placement_plugin_UIS} )
 
 include_directories(${base_placement_plugin_INCLUDE_DIRECTORIES} ${catkin_INCLUDE_DIRS})
 add_library(${PROJECT_NAME} ${base_placement_plugin_SRCS} ${base_placement_plugin_MOCS} ${base_placement_plugin_UIS_H}) # ${base_placement_plugin_UIS_H}
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}  yaml-cpp -lhdf5 -lhdf5_cpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}  yaml-cpp -lhdf5 -lhdf5_cpp  create_marker)
 ## target_link_libraries(${PROJECT_NAME} yaml-cpp)
 
+
+
 find_package(class_loader)
 class_loader_hide_library_symbols(${PROJECT_NAME})
 
@@ -90,3 +103,4 @@ install(PROGRAMS scripts/base_placement_plugin
 install(FILES plugin.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
+

base_placement_plugin/include/base_placement_plugin/add_robot_base.h

@@ -0,0 +1,117 @@
+#ifndef ADD_ROBOT_BASE_H
+#define ADD_ROBOT_BASE_H
+#ifndef Q_MOC_RUN
+#include<stdio.h>
+#include<iostream>
+#include <string.h>
+#include <math.h>
+#include <algorithm>
+#include <vector>
+#include <iterator>
+
+#include <rviz/panel.h>
+#include <ros/ros.h>
+#include <interactive_markers/interactive_marker_server.h>
+#include <interactive_markers/menu_handler.h>
+#include <rviz/default_plugin/interactive_markers/interactive_marker.h>
+#include <tf/LinearMath/Vector3.h>
+#include <tf/LinearMath/Scalar.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PointStamped.h>
+#include <std_msgs/ColorRGBA.h>
+#include <tf/tf.h>
+#include <tf/transform_listener.h>
+#include <tf/message_filter.h>
+#include <message_filters/subscriber.h>
+
+#include <rviz/properties/bool_property.h>
+#include <rviz/properties/string_property.h>
+#include <base_placement_plugin/widgets/base_placement_widget.h>
+#include <base_placement_plugin/place_base.h>
+
+#include <QWidget>
+#include <QCursor>
+#include <QObject>
+#include <QKeyEvent>
+#include <QHBoxLayout>
+#include <QTimer>
+#include <QtConcurrentRun>
+#include <QFuture>
+
+#endif
+
+
+namespace rviz
+{
+class VectorProperty;
+class VisualizationManager;
+class ViewportMouseEvent;
+class StringProperty;
+class BoolProperty;
+}
+
+
+class AddRobotBase : public QObject
+{
+    Q_OBJECT;
+  public:
+    AddRobotBase(QWidget* parent = 0);
+    virtual ~AddRobotBase();
+    void init();
+
+    virtual void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
+
+    void changeMarkerControlAndPose(std::string marker_name, bool set_control);
+    visualization_msgs::InteractiveMarkerControl& makeArrowControlDefault(visualization_msgs::InteractiveMarker& msg);
+    visualization_msgs::InteractiveMarkerControl& makeArrowControlDetails(visualization_msgs::InteractiveMarker& msg);
+    visualization_msgs::Marker makeWayPoint(visualization_msgs::InteractiveMarker& msg);
+    void pointDeleted(std::string marker_name);
+    void makeArrow(const tf::Transform& point_pos, int count_arrow);
+    void makeInteractiveMarker();
+    visualization_msgs::InteractiveMarkerControl& makeInteractiveMarkerControl(visualization_msgs::InteractiveMarker& msg);
+    visualization_msgs::Marker makeInterArrow(visualization_msgs::InteractiveMarker& msg);
+
+    void pointPoseUpdated(const tf::Transform& point_pos, const char* marker_name);
+    void getWaypoints(std::vector<geometry_msgs::Pose>& waypoints);
+
+
+    public Q_SLOTS:
+    void parseWayPoints();
+    void clearAllPointsRviz();
+
+    void getRobotModelFrame_slot(const tf::Transform end_effector);
+
+protected:
+    QWidget* widget_;
+
+    Q_SIGNALS:
+    void baseWayPoints_signal(std::vector<geometry_msgs::Pose> base_poses);
+
+private:
+    //! Define a server for the Interactive Markers.
+      boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server;
+      interactive_markers::MenuHandler menu_handler;
+
+      //! for arrows...........need to be modified
+      std_msgs::ColorRGBA WAY_POINT_COLOR;
+      std_msgs::ColorRGBA ARROW_INTER_COLOR;
+      geometry_msgs::Vector3 WAY_POINT_SCALE_CONTROL;
+      geometry_msgs::Vector3 ARROW_INTER_SCALE_CONTROL;
+      float INTERACTIVE_MARKER_SCALE;
+      float ARROW_INTERACTIVE_SCALE;
+
+      //! Vector for storing all the User Entered Way-Points.
+      std::vector< tf::Transform > waypoints_pos;
+      //! The position of the User handlable Interactive Marker.
+      tf::Transform box_pos;
+      int count;
+      std::string target_frame_;
+
+};
+
+
+
+
+
+
+#endif // ADD_ROBOT_BASE_H

base_placement_plugin/include/base_placement_plugin/add_way_point.h

@@ -29,6 +29,7 @@
 #include <base_placement_plugin/widgets/base_placement_widget.h>
 
 #include <base_placement_plugin/place_base.h>
+#include<base_placement_plugin/add_robot_base.h>
 
 #include <QWidget>
 #include <QCursor>

base_placement_plugin/include/base_placement_plugin/create_marker.h

@@ -0,0 +1,40 @@
+#ifndef CREATE_MARKER_H
+#define CREATE_MARKER_H
+
+#include<ros/ros.h>
+#include<geometry_msgs/Pose.h>
+#include<visualization_msgs/InteractiveMarker.h>
+#include<tf2/LinearMath/Transform.h>
+#include<tf2/LinearMath/Quaternion.h>
+
+#include<moveit/move_group_interface/move_group.h>
+
+#include<moveit/robot_model_loader/robot_model_loader.h>
+#include<moveit/robot_model/robot_model.h>
+#include<moveit/robot_model/joint_model_group.h>
+
+#include<moveit/robot_state/robot_state.h>
+
+typedef std::multimap<std::vector<double>,geometry_msgs::Pose> BasePoseJoint;
+
+class CreateMarker{
+public:
+  CreateMarker(std::string group_name);
+
+
+  bool makeArmMarker(BasePoseJoint baseJoints,  std::vector<visualization_msgs::InteractiveMarker>& iMarkers, bool show_unreachable_models); //visualizing robot model with joint solutions
+  bool makeRobotMarker(BasePoseJoint baseJoints,  std::vector<visualization_msgs::InteractiveMarker>& iMarkers, bool show_unreachable_models); //visualizing robot model with joint solutions
+  bool checkForEE();
+
+
+
+private:
+
+  std::string group_name_;
+  ros::AsyncSpinner spinner;
+  boost::scoped_ptr<moveit::planning_interface::MoveGroup> group_;
+  //Robot model cons pointer
+  moveit::core::RobotModelConstPtr robot_model_;
+};
+
+#endif // CREATE_MARKER_H

base_placement_plugin/include/base_placement_plugin/place_base.h

@@ -6,6 +6,16 @@
 #include <pluginlib/class_loader.h>
 #include <std_msgs/String.h>
 
+#include<moveit/planning_scene_monitor/planning_scene_monitor.h>
+#include<moveit/move_group_interface/move_group.h>
+#include<moveit/robot_model_loader/robot_model_loader.h>
+#include<moveit/robot_model/robot_model.h>
+#include<moveit/robot_model/joint_model_group.h>
+#include<moveit/robot_state/robot_state.h>
+
+#include<base_placement_plugin/add_robot_base.h>
+
+
 #include <QObject>
 #include <QTimer>
 #include <QtConcurrentRun>
@@ -54,8 +64,11 @@ public Q_SLOTS:
   //! Showing the base locations by robot model
   void showBaseLocationsbyRobotModel(std::vector< geometry_msgs::Pose > po);
 
+  //! Showing the base locations by only arm
+  void showBaseLocationsbyArmModel(std::vector< geometry_msgs::Pose > po);
+
   //! Get the Way-Points from the RViz enviroment and use them to find base.
-  void findbase(std::vector< geometry_msgs::Pose > grasp_poses);
+  bool findbase(std::vector< geometry_msgs::Pose > grasp_poses);
 
   //! Sending the initial marker location and list of base Placement Method and visualization methods
   void initRvizDone();
@@ -72,28 +85,46 @@ public Q_SLOTS:
   //! Getting user Defined visualization method
   void getSelectedOpType(int op_index);
 
+  //! Getting user Defined robot model
+  void getSelectedRobotGroup(int model_index);
+
+  //! Getting user Defined unreachable model shown method
+  void getShowUreachModels(bool show_u_models);
+
+  //!Getting the user Defined base poses
+  void  getBasePoses(std::vector<geometry_msgs::Pose> base_poses);
+
+
+
 Q_SIGNALS:
 
   //! Signal for initial marker frame
   void getinitialmarkerFrame_signal(const tf::Transform trns);
 
   //! Let the RQT Widget know that a Base Placement Process has started.
   void basePlacementProcessStarted();
-  ;
+
 
   //! Let the RQT Widget know that Base Placement Process has finished.
   void basePlacementProcessFinished();
 
   //! Let the RQT Widget know that Base Placement Process completed so it can show finish message
-  void basePlacementProcessCompleted();
+  void basePlacementProcessCompleted(double score);
 
   //! Send the Method groups to the GUI
   void sendBasePlaceMethods_signal(std::vector< std::string > method_names);
 
   //! Send the output types to the GUI
   void sendOuputType_signal(std::vector< std::string > output_type);
 
+  //! Send the output types to the GUI
+  void sendGroupType_signal(std::vector< std::string > group_names);
+
+
+
 protected:
+  //QObject* add_robot;
+  //AddRobotBase add_robot;
   //! Base Placement by PCA
   void findBaseByPCA();
 
@@ -103,14 +134,38 @@ public Q_SLOTS:
   //!Base Placement by IKSolutionScore
   void findBaseByIKSolutionScore();
 
+  //!Base Placement by VerticalRobotModel
+  void findBaseByVerticalRobotModel();
+
+  //!Base Placement by VerticalRobotModel
+  void findBaseByUserIntuition();
+
   //! Selecting the Base Placement Method
   void BasePlaceMethodHandler();
 
   //! Selecting the Visualization Method
   void OuputputVizHandler(std::vector< geometry_msgs::Pose > po);
 
+
+
+  //Transforming reachability data towards robot base
+  void transformToRobotbase(std::multimap< std::vector< double >, std::vector< double > > armBasePoses,
+                            std::multimap< std::vector< double >, std::vector< double > >& robotBasePoses);
+
+  void transformFromRobotbaseToArmBase(const geometry_msgs::Pose& base_pose, geometry_msgs::Pose &arm_base_pose);
+  void createSpheres(std::multimap< std::vector< double >, std::vector< double > > basePoses,
+                     std::map< std::vector< double >, double >& spColor, std::vector< std::vector< double > >& highScoredSp, bool reduce_D);
+
+  double calculateScoreForRobotBase(std::vector<geometry_msgs::Pose>& grasp_poses, std::vector<geometry_msgs::Pose>& base_poses);
+  double calculateScoreForArmBase(std::vector<geometry_msgs::Pose>& grasp_poses, std::vector<geometry_msgs::Pose>& base_poses);
+
+  void loadRobotModel();
+
+  void getRobotGroups(std::vector<std::string>& groups);
+
   int selected_method_;
   int selected_op_type_;
+  std::string selected_group_;
 
   //! Number of base place locations to show
   int BASE_LOC_SIZE_;
@@ -121,6 +176,8 @@ public Q_SLOTS:
   std::vector< std::string > method_names_;
   //! Vector for output visualization
   std::vector< std::string > output_type_;
+  //! Vector for robot groups
+  std::vector<std::string> group_names_;
   //! Taking the reachability data from the file
   std::multimap< std::vector< double >, std::vector< double > > PoseColFilter;
   std::multimap< std::vector< double >, double > SphereCol;
@@ -134,9 +191,23 @@ public Q_SLOTS:
   std::vector< std::vector< double > > highScoreSp;
   //! Vector for storing final base poses
   std::vector< geometry_msgs::Pose > final_base_poses;
+  std::vector<geometry_msgs::Pose> final_base_poses_user;
 
   //! Vector for grasp poses
   std::vector< geometry_msgs::Pose > GRASP_POSES_;
+
+
+  //! Reachability data for the union map
+  std::multimap< std::vector< double >, std::vector< double > > robot_PoseColfilter;
+
+  double score_;
+  geometry_msgs::Pose best_pose_;
+
+  //Robot model cons pointer
+  moveit::core::RobotModelConstPtr robot_model_;
+
+  //show unreachable models
+  bool show_ureach_models_;
 };
 
 #endif  // PLACE_BASE_H_