robot visualization improved

Author: jontromanab

base_placement_plugin/include/base_placement_plugin/create_marker.h

@@ -6,14 +6,17 @@
 #include<visualization_msgs/InteractiveMarker.h>
 #include<tf2/LinearMath/Transform.h>
 #include<tf2/LinearMath/Quaternion.h>
+#include <Eigen/Eigen>
+#include <eigen_conversions/eigen_msg.h>
 
 #include<moveit/move_group_interface/move_group.h>
+#include<moveit/robot_state/robot_state.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_model/link_model.h>
 
-#include<moveit/robot_state/robot_state.h>
 
 typedef std::multimap<std::vector<double>,geometry_msgs::Pose> BasePoseJoint;
 
@@ -24,17 +27,33 @@ class CreateMarker{
 
   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();
+  bool checkEndEffector();
+
+
 
 
 
 private:
 
+  void discardUnreachableModels(BasePoseJoint& baseJoints);
+  void makeIntMarkers(BasePoseJoint& basePJoints, bool arm_only, std::vector<visualization_msgs::InteractiveMarker> &iMarkers);
+  void updateRobotState(const std::vector<double>& joint_soln, moveit::core::RobotStatePtr robot_state);
+  void getFullLinkNames(std::vector<std::string>& full_link_names, bool arm_only);
+  void getArmLinks(std::vector<std::string>& arm_links);
+  void getEELinks(std::vector<std::string>& ee_links);
+  void makeIntMarkerControl(const geometry_msgs::Pose& base_pose, const std::vector<double>& joint_soln,bool arm_only, bool is_reachable, visualization_msgs::InteractiveMarkerControl& robotModelControl);
+  void createInteractiveMarker(const geometry_msgs::Pose& base_pose, const std::vector<double>& joint_soln,
+                                             const int& num, bool arm_only, bool is_reachable,visualization_msgs::InteractiveMarker& iMarker);
+
+  void updateMarkers(const geometry_msgs::Pose& base_pose, bool is_reachable, Eigen::Affine3d tf_first_link_to_root, visualization_msgs::MarkerArray& markers);
+
+
+
   std::string group_name_;
   ros::AsyncSpinner spinner;
   boost::scoped_ptr<moveit::planning_interface::MoveGroup> group_;
-  //Robot model cons pointer
-  moveit::core::RobotModelConstPtr robot_model_;
+  std::string parent_link;
+  moveit::core::RobotModelConstPtr robot_model_; //Robot model const pointer
 };
 
 #endif // CREATE_MARKER_H

base_placement_plugin/include/base_placement_plugin/place_base.h

@@ -5,6 +5,7 @@
 
 #include <pluginlib/class_loader.h>
 #include <std_msgs/String.h>
+#include<base_placement_plugin/create_marker.h>
 
 #include<moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include<moveit/move_group_interface/move_group.h>
@@ -159,7 +160,8 @@ public Q_SLOTS:
   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();
+  bool loadRobotModel();
+  bool checkforRobotModel();
 
   void getRobotGroups(std::vector<std::string>& groups);
 
@@ -208,6 +210,9 @@ public Q_SLOTS:
 
   //show unreachable models
   bool show_ureach_models_;
+
+
+  CreateMarker* mark_;
 };
 
 #endif  // PLACE_BASE_H_

base_placement_plugin/launch/base_placement.launch

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
 
-<arg name = "moveit_pkg" default="ur5_moveit_config"/>
+<arg name = "moveit_pkg" default="motoman_moveit_config"/>
 
 <!--Launch moveit required pkgs for your robot-->
 <include file="$(find base_placement_plugin)/launch/moveit_pkgs.launch">