Obbart/RoboGlue_ROS
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Prima implementazione dell'homing, i giunti vanno tutti a 0

Browse Source
This commit is contained in:
Emanuele
2019-10-23 15:43:10 +02:00
parent 192d694458
commit 6f1f4820a9
5 changed files with 34 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/roboglue_ros/include/roboglue_ros/roboglue_utils.h
View File

@@ -97,7 +97,7 @@ typedef struct {
robot_model::RobotModelConstPtr kine_model;
robot_state::RobotStatePtr kine_state;
std::vector<std::string> joint_names;
std::map<double> joint_homes;
std::map<std::string, double> joint_homes;
} robotData;
typedef struct {
@@ -112,4 +112,5 @@ geometry_msgs::TwistStamped lockTwistPose(geometry_msgs::TwistStamped, geometry_
geometry_msgs::PoseStamped twist2Pose(geometry_msgs::TwistStamped);
double e3dist(geometry_msgs::Twist, geometry_msgs::Twist);
double rad2deg(double ang);
double deg2rad(double ang);
bool checkJointJump(moveit_msgs::RobotTrajectoryPtr, double);

38
src/roboglue_ros/src/roboglue_follower.cpp
View File

@@ -22,12 +22,12 @@
//////////////////////////////////////////
void testFunction( ros::Publisher* trj_pub);
trajectory_msgs::JointTrajectory composeTrjMessage(std::vector<double> jtpos);
bool resetModelJointsHome(geometry_msgs::Pose, bool);
void resetModelJointsHome(robotData*);
///////////////////////////////////////////
///////// ROS CALLBACKS //////////////////
/////////////////////////////////////////
void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, positionData* pos) {
void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, publisherMap* pubs, positionData* pos, robotData* rdata) {
ROS_DEBUG("Received command: [%s]", msg->data.c_str());
try {
nlohmann::json c = nlohmann::json::parse(msg->data.c_str());
@@ -47,9 +47,22 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
}
else ROS_ERROR("Invalid acton");
} else if (!command.compare("RECORD") || !command.compare("PLAY") || !command.compare("OPEN")) {
return;
} else if (!command.compare("HOME")) {
// Chiamata alla funzione di reset dei giunti nel modello
// Chiamata alla funzione di reset dei giunti nel modello
resetModelJointsHome(rdata);
if (!params["execute"].get<bool>()){
ROS_DEBUG("Autosetting joints to Home Position");
ROS_WARN("Remote execution of homing not SAFE yet!!");
std::vector<double> jPos;
rdata->kine_state->copyJointGroupPositions(rdata->joint_modelGroup, jPos);
for (std::size_t i = 0; i < rdata->joint_names.size(); ++i) {
ROS_DEBUG_COND(true,"Joint %s: %f", rdata->joint_names[i].c_str(), rad2deg(jPos[i]));
}
pubs->at("trj_pub")->publish(composeTrjMessage(jPos));
} else {
ROS_WARN("Joint model state set to Home, robot still in last position!!");
}
} else {
ROS_WARN("Unknown command: %s", command.c_str());
}
@@ -199,7 +212,7 @@ int main(int argc, char **argv) {
/// set spinner rate ///
ros::Rate loop_rate(loopRate);
/// set console log level ///
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
ROS_INFO("Follower Node Started");
/// advertise publish topics ///
@@ -216,7 +229,7 @@ int main(int argc, char **argv) {
publishers["trj_pub"] = &trj_pub;
/// subscribe to incoming topics ///
ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(ros_topics.commandSub, static_cast<uint32_t>(msgBufferSize),
boost::bind(commandCallback, _1, &is, &pos_data));
boost::bind(commandCallback, _1, &is, &publishers, &pos_data, &robot_data));
ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(ros_topics.coordSub, static_cast<uint32_t>(msgBufferSize),
boost::bind(coordinateCallback, _1, &publishers, &pos_data));
ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.stateSub, static_cast<uint32_t>(msgBufferSize),
@@ -249,16 +262,17 @@ int main(int argc, char **argv) {
///////////// END MAIN /////////////////
////////////////////////////////////////
// TODO: aggiungere la possibilità di resettare i giunti nella configurazione home a comando da interfaccia
bool resetModelJointsHome(geometry_msgs::Pose home_pos, bool execute=false) {
void resetModelJointsHome(robotData* rdata) {
ROS_DEBUG("Setting joint position to home");
if (execute){
return false;
} else {
return false;
std::map<std::string, double> jhome = rdata->joint_homes;
for (auto joint : jhome) {
std::string jname = joint.first;
const double jval = deg2rad(joint.second);
rdata->kine_state->setJointPositions(jname, &jval);
}
}
//TODO: rendere dinamici i nomi dei giunti caricati in robot_data
trajectory_msgs::JointTrajectory composeTrjMessage(std::vector<double> jtpos){
static trajectory_msgs::JointTrajectory trj;
static double ang=0.0;

2
src/roboglue_ros/src/roboglue_recorder.cpp
View File

@@ -262,7 +262,7 @@ int main(int argc, char **argv) {
nh.getParam(param_ns+"move_group_name", robot_data.move_group_name);
/// set console log level ///
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn);
/// set spinner rate ///
ros::Rate loop_rate(loopRate);

2
src/roboglue_ros/src/roboglue_relay.cpp
View File

@@ -108,7 +108,7 @@ int main(int argc, char **argv) {
/// set spinner rate ///
ros::Rate loop_rate(loopRate);
/// set console log level ///
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn);
ROS_INFO("Relay Node Started");
/// initialize MQTT client ///
mqtt::async_client client(mqtt_topics.mqttHost,"");

4
src/roboglue_ros/src/roboglue_utils.cpp
View File

@@ -82,6 +82,10 @@ double rad2deg(double ang){
return ang*180/M_PI;
}
double deg2rad(double ang){
return ang*M_PI/180;
}
bool checkJointJump(moveit_msgs::RobotTrajectoryPtr traj, double thresh){
// identify max joint jump for each joint
typedef struct{