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

Homing funzionante, da perfezionare.

Browse Source 
Tutte le posizioni di default fanno parte del file .yaml
This commit is contained in:
Emanuele
2019-10-23 17:37:46 +02:00
parent c799003f93
commit cee517fd2c
3 changed files with 25 additions and 28 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<std::string, double> joint_homes;
std::map<std::string, float> joint_homes;
} robotData;
typedef struct {
@@ -110,6 +110,7 @@ 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);

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

@@ -21,8 +21,7 @@
///////// FUNCTION DECLARATIONS ///////////
//////////////////////////////////////////
void testFunction( ros::Publisher* trj_pub);
trajectory_msgs::JointTrajectory composeTrjMessage(std::vector<double> jtpos);
void resetModelJointsHome(robotData*);
trajectory_msgs::JointTrajectory composeTrjMessage(std::vector<double> jtpos, std::vector<std::string> jnames);
///////////////////////////////////////////
///////// ROS CALLBACKS //////////////////
@@ -47,20 +46,19 @@ 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
resetModelJointsHome(rdata);
rdata->kine_state->setVariablePositions(rdata->joint_homes);
rdata->kine_state->setVariablePositions(floatMap2doubleMap(rdata->joint_homes));
if (!params["execute"].get<bool>()){
ROS_DEBUG("Autosetting joints to Home Position");
ROS_WARN("Remote execution of homing not SAFE yet!!");
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));
pubs->at("trj_pub")->publish(composeTrjMessage(jPos, rdata->joint_names));
} else {
ROS_WARN("Joint model state set to Home, robot still in last position!!");
}
@@ -73,7 +71,7 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
}
void coordinateCallback(const std_msgs::String::ConstPtr& msg, publisherMap* pubs, positionData* pos){
ROS_DEBUG("Received coordinate: [%s]", msg->data.c_str());
ROS_DEBUG_COND(false, "Received coordinate: [%s]", msg->data.c_str());
nlohmann::json coordinateParsed = nlohmann::json::parse(msg->data);
std::string type = coordinateParsed["type"];
std::string mode = coordinateParsed["mode"];
@@ -126,7 +124,7 @@ void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMa
double timeout = 0.1;
uint attempts = 10;
bool foundIK = false;
std::vector<double> jPos, jStartPos = {M_PI/2, -3*M_PI/4, +3*M_PI/4, -M_PI/2, -M_PI/2, 0};
std::vector<double> jPos;
if (is->isRealtime){
twPose = pos->twist;
@@ -141,17 +139,17 @@ void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMa
newPose.pose.position.x, newPose.pose.position.y, newPose.pose.position.z,
newPose.pose.orientation.x, newPose.pose.orientation.y, newPose.pose.orientation.z, newPose.pose.orientation.w);
if (firstIK) {
rdata->kine_state->setVariablePositions(jStartPos);
rdata->kine_state->setVariablePositions(floatMap2doubleMap(rdata->joint_homes));
for (auto jj : rdata->kine_state->getVariableNames())
ROS_DEBUG("jointname: %s", jj.c_str());
rdata->kine_state->copyJointGroupPositions(rdata->joint_modelGroup, jStartPos);
rdata->kine_state->copyJointGroupPositions(rdata->joint_modelGroup, jPos);
rdata->kine_state->updateLinkTransforms();
ROS_DEBUG_STREAM("Cart Start Position: " << rdata->kine_state->getGlobalLinkTransform("ee_link").translation() << std::endl);
ROS_DEBUG_COND(true, "Joint Start Position: J1:%6.3f, J2:%6.3f, J3:%6.3f \n\
J4:%6.3f, J5:%6.3f, J6:%6.3f",
jStartPos[0], jStartPos[1],jStartPos[2],
jStartPos[3], jStartPos[4],jStartPos[5]);
rdata->kine_state->setJointGroupPositions(rdata->joint_modelGroup, jStartPos);
jPos[0], jPos[1],jPos[2],
jPos[3], jPos[4],jPos[5]);
rdata->kine_state->setVariablePositions(floatMap2doubleMap(rdata->joint_homes));
firstIK = false;
}
foundIK = rdata->kine_state->setFromIK(rdata->joint_modelGroup, newPose.pose, attempts, timeout);
@@ -160,7 +158,7 @@ void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMa
for (std::size_t i = 0; i < rdata->joint_names.size(); ++i) {
ROS_DEBUG_COND(false,"Joint %s: %f", rdata->joint_names[i].c_str(), rad2deg(jPos[i]));
}
pubs->at("trj_pub")->publish(composeTrjMessage(jPos));
pubs->at("trj_pub")->publish(composeTrjMessage(jPos, rdata->joint_names));
} else {
ROS_WARN("No IK solution found");
}
@@ -262,23 +260,13 @@ int main(int argc, char **argv) {
////////////////////////////////////////
///////////// END MAIN /////////////////
////////////////////////////////////////
void resetModelJointsHome(robotData* rdata) {
ROS_DEBUG("Setting joint position to home");
for(auto joint : rdata->joint_homes){
joint.second = deg2rad(joint.second);
rdata->joint_homes[joint.first] = joint.second;
}
}
//TODO: rendere dinamici i nomi dei giunti caricati in robot_data
trajectory_msgs::JointTrajectory composeTrjMessage(std::vector<double> jtpos){
trajectory_msgs::JointTrajectory composeTrjMessage(std::vector<double> jtpos, std::vector<std::string> jnames){
static trajectory_msgs::JointTrajectory trj;
static double ang=0.0;
trj.header.seq++;
trj.header.stamp = ros::Time::now();
trj.points.clear();
trj.joint_names = {"shoulder_pan_joint","shoulder_lift_joint","elbow_joint","wrist_1_joint","wrist_2_joint","wrist_3_joint"};
trj.joint_names = jnames;
trajectory_msgs::JointTrajectoryPoint pt;
pt.positions = jtpos;
pt.time_from_start.fromSec(0.5);

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

@@ -70,6 +70,14 @@ geometry_msgs::PoseStamped twist2Pose(geometry_msgs::TwistStamped twt){
return newPoseStamp;
}
std::map<std::string, double> floatMap2doubleMap(std::map<std::string,float> m) {
std::map<std::string, double> tmp;
for (auto j : m){
tmp[j.first] = (deg2rad(static_cast<double>(j.second)));
}
return tmp;
}
double e3dist(geometry_msgs::Twist p1, geometry_msgs::Twist p2){
double res;
res = pow(p1.linear.x - p2.linear.x, 2.0) +