imarker_robot_state.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2016, University of Colorado, Boulder
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Univ of Colorado nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman
36  Desc: Class to encapsule a visualized robot state that can be controlled using an interactive marker
37 */
38 
39 #ifndef MOVEIT_VISUAL_TOOLS_IMARKER_ROBOT_STATE_H
40 #define MOVEIT_VISUAL_TOOLS_IMARKER_ROBOT_STATE_H
41 
42 // ROS
43 #include <ros/ros.h>
45 #include <visualization_msgs/InteractiveMarkerFeedback.h>
46 
47 // MoveIt
50 
51 // C++
52 #include <string>
53 #include <vector>
54 
55 namespace moveit_visual_tools
56 {
57 using visualization_msgs::InteractiveMarkerFeedback;
58 using visualization_msgs::InteractiveMarkerControl;
59 
60 typedef std::function<void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr&, const Eigen::Affine3d&)>
62 
63 typedef std::shared_ptr<interactive_markers::InteractiveMarkerServer> InteractiveMarkerServerPtr;
64 
65 class IMarkerEndEffector;
66 typedef std::shared_ptr<IMarkerEndEffector> IMarkerEndEffectorPtr;
67 typedef std::shared_ptr<const IMarkerEndEffector> IMarkerEndEffectorConstPtr;
68 
72 struct ArmData
73 {
75 
78 };
79 
81 {
82  friend class IMarkerEndEffector;
83 
84 public:
88  IMarkerRobotState(planning_scene_monitor::PlanningSceneMonitorPtr psm, const std::string& imarker_name,
89  std::vector<ArmData> arm_datas, rviz_visual_tools::colors color, const std::string& package_path);
90 
92  {
93  output_file_.close();
94  }
95 
96  bool loadFromFile(const std::string& file_name);
97 
98  bool saveToFile();
99 
101  void setIMarkerCallback(IMarkerCallback callback);
102 
104  moveit::core::RobotStateConstPtr getRobotState()
105  {
106  return imarker_state_;
107  }
108  moveit::core::RobotStatePtr getRobotStateNonConst()
109  {
110  return imarker_state_;
111  }
112 
114  void setRobotState(moveit::core::RobotStatePtr state);
115 
117  void setToCurrentState();
118 
124  bool setToRandomState(double clearance = 0);
125 
127  bool isStateValid(bool verbose = false);
128 
130  void publishRobotState();
131 
133 
134  bool getFilePath(std::string& file_path, const std::string& file_name, const std::string& subdirectory) const;
135 
136  IMarkerEndEffectorPtr getEEF(const std::string& name)
137  {
138  return name_to_eef_[name];
139  }
140 
141  bool setFromPoses(const EigenSTL::vector_Affine3d poses, const moveit::core::JointModelGroup* group);
142 
143 protected:
144  // --------------------------------------------------------
145 
146  // The short name of this class
147  std::string name_;
148 
149  // A shared node handle
151 
152  // State
153  moveit::core::RobotStatePtr imarker_state_;
154 
155  // End effectors
156  std::vector<ArmData> arm_datas_;
157  std::vector<IMarkerEndEffectorPtr> end_effectors_;
158  std::map<std::string, IMarkerEndEffectorPtr> name_to_eef_;
159 
160  // Core MoveIt components
161  planning_scene_monitor::PlanningSceneMonitorPtr psm_;
162 
163  // Visual tools
165 
166  // Settings
167  std::size_t refresh_rate_ = 30;
169 
170  // Interactive markers
171  InteractiveMarkerServerPtr imarker_server_;
172 
173  // File saving
174  std::string file_path_;
175  std::ofstream output_file_;
176  std::string package_path_; // File location of this package
177 
178 }; // end class
179 
180 // Create std pointers for this class
181 typedef std::shared_ptr<IMarkerRobotState> IMarkerRobotStatePtr;
182 typedef std::shared_ptr<const IMarkerRobotState> IMarkerRobotStateConstPtr;
183 } // namespace moveit_visual_tools
184 
185 namespace
186 {
188 bool isIKStateValid(const planning_scene::PlanningScene* planning_scene, bool verbose, bool only_check_self_collision,
189  moveit_visual_tools::MoveItVisualToolsPtr visual_tools_, robot_state::RobotState* state,
190  const robot_state::JointModelGroup* group, const double* ik_solution);
191 }
192 
193 #endif // MOVEIT_VISUAL_TOOLS_IMARKER_ROBOT_STATE_H
IMarkerEndEffectorPtr getEEF(const std::string &name)
std::shared_ptr< const IMarkerEndEffector > IMarkerEndEffectorConstPtr
std::shared_ptr< MoveItVisualTools > MoveItVisualToolsPtr
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
std::function< void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &, const Eigen::Affine3d &)> IMarkerCallback
std::vector< IMarkerEndEffectorPtr > end_effectors_
std::shared_ptr< IMarkerEndEffector > IMarkerEndEffectorPtr
InteractiveMarkerServerPtr imarker_server_
bool verbose
moveit::core::JointModelGroup * jmg_
ArmData(moveit::core::JointModelGroup *jmg, moveit::core::LinkModel *ee_link)
std::map< std::string, IMarkerEndEffectorPtr > name_to_eef_
planning_scene_monitor::PlanningSceneMonitorPtr psm_
moveit::core::RobotStatePtr imarker_state_
std::shared_ptr< const IMarkerRobotState > IMarkerRobotStateConstPtr
I wish this struct wasn&#39;t necessary, but the SRDF does not seem to support choosing one particular li...
moveit::core::RobotStateConstPtr getRobotState()
Get a pointer to the current robot state.
moveit::core::RobotStatePtr getRobotStateNonConst()
moveit_visual_tools::MoveItVisualToolsPtr visual_tools_
std::shared_ptr< interactive_markers::InteractiveMarkerServer > InteractiveMarkerServerPtr
moveit::core::LinkModel * ee_link_
std::shared_ptr< IMarkerRobotState > IMarkerRobotStatePtr


moveit_visual_tools
Author(s): Dave Coleman
autogenerated on Thu Jun 6 2019 19:40:28