118 lines
3.5 KiB
C++
118 lines
3.5 KiB
C++
#ifndef ROBOGLUE_UTILS_H
|
|
#define ROBOGLUE_UTILS_H
|
|
|
|
#endif // ROBOGLUE_UTILS_H
|
|
|
|
#include <iostream>
|
|
#include <stdlib.h>
|
|
#include <ros/ros.h>
|
|
#include <boost/bind.hpp>
|
|
#include <boost/algorithm/string.hpp>
|
|
#include <std_msgs/String.h>
|
|
#include <eigen_conversions/eigen_msg.h>
|
|
#include <geometry_msgs/PoseStamped.h>
|
|
#include <geometry_msgs/TwistStamped.h>
|
|
#include <trajectory_msgs/JointTrajectory.h>
|
|
|
|
#include <moveit/move_group_interface/move_group_interface.h>
|
|
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
|
#include <moveit/robot_model_loader/robot_model_loader.h>
|
|
#include <moveit/robot_model/robot_model.h>
|
|
#include <moveit/robot_state/robot_state.h>
|
|
#include <moveit_visual_tools/moveit_visual_tools.h>
|
|
|
|
#include <Eigen/Eigen>
|
|
#include <tf/tf.h>
|
|
|
|
#include <roboglue_ros/json.hpp>
|
|
#include <roboglue_ros/rapidcsv.h>
|
|
|
|
typedef std::map<std::string, ros::Publisher*> publisherMap;
|
|
|
|
/// setup rviz palnned path visualization
|
|
namespace rvt=rviz_visual_tools;
|
|
namespace mvt=moveit_visual_tools;
|
|
|
|
///////// MQTT & ROS TOPIC NAMES ///////////////
|
|
typedef struct {
|
|
std::string MQTTcommandPub, MQTTcoordPub, MQTTstatePub;
|
|
std::string MQTTcommandSub, MQTTcoordSub, MQTTstateSub;
|
|
std::string mqttHost;
|
|
int mqttQoS;
|
|
} MQTT_topics;
|
|
|
|
typedef struct {
|
|
std::string commandPub, coordPub, statePub;
|
|
std::string commandSub, coordSub, stateSub;
|
|
std::string delta_jog, delta_joint_jog, traj_jog;
|
|
} ROS_topics;
|
|
|
|
///////// AUX DATA STORAGE ///////////////
|
|
typedef struct {
|
|
std::list<std::pair<std::string,bool>> digital;
|
|
std::list<std::pair<std::string,uint16_t>> analog;
|
|
} auxData;
|
|
|
|
////////// PLAY & RECORD DATA SORAGE ////////
|
|
typedef struct {
|
|
geometry_msgs::TwistStamped point;
|
|
size_t pointNumber;
|
|
double dS;
|
|
auxData pointAux;
|
|
} pointRecord;
|
|
|
|
////////// INTERNAL STATE STORAGE //////////
|
|
typedef struct{
|
|
bool isFileOpen = false;
|
|
bool isPlaying = false;
|
|
bool isRecording = false;
|
|
bool isRealtime = false;
|
|
bool isRunning = true;
|
|
} internalState;
|
|
|
|
////////// POSITION DATA STORAGE ///////////
|
|
typedef struct {
|
|
geometry_msgs::TwistStamped twist;
|
|
trajectory_msgs::JointTrajectory joint;
|
|
bool waitIK = false;
|
|
bool isFirst = false;
|
|
bool isNew = false;
|
|
bool isJoint = false;
|
|
bool isTwist = false;
|
|
bool isVel = false;
|
|
bool isPos = false;
|
|
bool lockX = false;
|
|
bool lockY = false;
|
|
bool lockZ = false;
|
|
bool lockRX = false;
|
|
bool lockRY = false;
|
|
bool lockRZ = false;
|
|
} positionData;
|
|
|
|
///////// ROBOT DATA STORAGE /////////////
|
|
typedef struct {
|
|
std::string robot_model_name, move_group_name, base_frame;
|
|
std::string point_group_mode, planning_mode;
|
|
const robot_state::JointModelGroup* joint_modelGroup;
|
|
robot_model::RobotModelConstPtr kine_model;
|
|
robot_state::RobotStatePtr kine_state;
|
|
std::vector<std::string> joint_names;
|
|
std::map<std::string, float> joint_homes;
|
|
} robotData;
|
|
|
|
typedef struct {
|
|
mvt::MoveItVisualToolsPtr vtools;
|
|
} rvizData;
|
|
|
|
geometry_msgs::TwistStamped jstr2Twist(nlohmann::json);
|
|
trajectory_msgs::JointTrajectory jstr2Joint(nlohmann::json);
|
|
|
|
void getLockInfo(positionData*, nlohmann::json);
|
|
geometry_msgs::TwistStamped lockTwistPose(geometry_msgs::TwistStamped, geometry_msgs::TwistStamped, positionData);
|
|
geometry_msgs::PoseStamped twist2Pose(geometry_msgs::TwistStamped);
|
|
std::map<std::string, double> floatMap2doubleMap(std::map<std::string,float>);
|
|
double e3dist(geometry_msgs::Twist, geometry_msgs::Twist);
|
|
double rad2deg(double ang);
|
|
double deg2rad(double ang);
|
|
bool checkJointJump(moveit_msgs::RobotTrajectoryPtr, double);
|