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

Inizia implementazione Set Frame

Browse Source
This commit is contained in:
Emanuele
2019-11-12 10:51:15 +01:00
parent a0a36a310d
commit b7f1857e5c
5 changed files with 30 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
roboglue_ros_ws.workspace.user
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 4.9.1, 2019-11-05T16:47:35. -->
<!-- Written by QtCreator 4.9.1, 2019-11-12T09:36:03. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
@@ -124,7 +124,7 @@
<value type="QString">ROS_PACKAGE_PATH=/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src:/opt/ros/kinetic/share</value>
<value type="QString">ROS_ROOT=/opt/ros/kinetic/share/ros</value>
<value type="QString">ROS_VERSION=1</value>
<value type="QString">SHLVL=666</value>
<value type="QString">SHLVL=668</value>
<value type="QString">TERM=xterm</value>
<value type="QString">_=/usr/bin/env</value>
</valuelist>

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

@@ -54,6 +54,7 @@ typedef struct {
int loopRate, msgBufferSize;
std::string param_ns;
std_msgs::String startupMsg, shutdownMsg;
std::string defaultFrameName;
} COMMON_parameters;
///////// AUX DATA STORAGE ///////////////
@@ -83,6 +84,8 @@ typedef struct{
typedef struct {
geometry_msgs::TwistStamped twist;
trajectory_msgs::JointTrajectory joint;
geometry_msgs::Pose activeFrame;
std::string activeFrameName;
bool waitIK = false;
bool isFirst = false;
bool isNew = false;

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

@@ -226,7 +226,7 @@ int main(int argc, char **argv) {
/// set spinner rate ///
ros::Rate loop_rate(common_par.loopRate);
/// 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::Info);
ROS_INFO("Follower Node Started");
/// subscribe to incoming topics ///

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

@@ -87,6 +87,22 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
} else if (!action.compare("stop")) {
is->isPlaying = !stopPlay(is, fd);
}
} else if (!command.compare("SETFRAME")){
action = params["action"];
if (!action.compare("reset")){
pos->activeFrameName="/world";
pos->activeFrame=geometry_msgs::Pose();
} else if (!action.compare("set")){
pos->activeFrameName = params["framename"];
geometry_msgs::TwistStamped tempFrame;
tempFrame.twist.linear.x = params["frame"]["x"];
tempFrame.twist.linear.y = params["frame"]["y"];
tempFrame.twist.linear.z = params["frame"]["z"];
tempFrame.twist.angular.x = params["frame"]["rx"];
tempFrame.twist.angular.y = params["frame"]["ry"];
tempFrame.twist.angular.z = params["frame"]["rz"];
pos->activeFrame = twist2Pose(tempFrame).pose;
}
} else if (!command.compare("RECORD")){
action = params["action"];
if (!action.compare("start")){
@@ -248,6 +264,8 @@ int main(int argc, char **argv) {
nh.getParam(common_par.param_ns+"robot_model_name", robot_data.robot_model_name);
nh.getParam(common_par.param_ns+"move_group_name", robot_data.move_group_name);
nh.getParam(common_par.param_ns+"joint_jump_tresh", robot_data.joint_jump_tresh);
// load default frame parameters
nh.getParam(common_par.param_ns+"default_frame_name", pos_data.activeFrameName);
// load spin rate
nh.getParam("/roboglue_ros_recorder/loop_rate", common_par.loopRate);
//load meta template filename
@@ -264,7 +282,7 @@ int main(int argc, char **argv) {
nh.getParam("/roboglue_ros_recorder/planning_mode", robot_data.planning_mode);
/// 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::Info);
/// set spinner rate ///
ros::Rate loop_rate(common_par.loopRate);
ROS_INFO("Recorder Node Started");
@@ -305,6 +323,7 @@ int main(int argc, char **argv) {
robot_data.kine_state = std::make_shared<robot_state::RobotState>(robot_data.kine_model);
robot_data.joint_modelGroup = robot_data.kine_state->getJointModelGroup(robot_data.move_group_name);
robot_data.joint_names = robot_data.joint_modelGroup->getVariableNames();
move_group.setPoseReferenceFrame(pos_data.activeFrameName);
ROS_INFO("Planning in Frame: %s for link: %s", move_group.getPlanningFrame().c_str(), move_group.getEndEffectorLink().c_str());
ROS_INFO("IK calc in Frame: %s", robot_data.kine_model->getModelFrame().c_str());
@@ -332,6 +351,8 @@ int main(int argc, char **argv) {
/////////// PLAY FILE ///////////////////
/////////////////////////////////////////
if (is.isPlaying){
// verify execution frame and set to default if needed
move_group.setPoseReferenceFrame(pos_data.activeFrameName);
// i -> row counter for temp vector planning in both cartesian and joint mode
ROS_INFO_COND(i==0,"Start Play file: [%s]", file_data.metaFile->at("name").get<std::string>().c_str());
if (i < playVector.size()){
@@ -361,7 +382,7 @@ int main(int argc, char **argv) {
tempPlanningVector.clear();
for (auto cc : tempPlayVector) {
tempPlanningVector.push_back(twist2Pose(cc.point));
tempPlanningVector.back().header.frame_id="/world";
tempPlanningVector.back().header.frame_id=pos_data.activeFrameName;
}
//TODO sincronizzazione delle uscite ausiliarie con il movimento del robot!!!
//TODO l'invio di traiettorie segmentato in più esecuzioni funziona

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

@@ -121,7 +121,7 @@ int main(int argc, char **argv) {
/// set spinner rate ///
ros::Rate loop_rate(common_par.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,"");