diff --git a/roboglue_ros_ws.workspace.user b/roboglue_ros_ws.workspace.user index 94273c8..53e2700 100644 --- a/roboglue_ros_ws.workspace.user +++ b/roboglue_ros_ws.workspace.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -124,7 +124,7 @@ ROS_PACKAGE_PATH=/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src:/opt/ros/kinetic/share ROS_ROOT=/opt/ros/kinetic/share/ros ROS_VERSION=1 - SHLVL=786 + SHLVL=812 TERM=xterm _=/usr/bin/env @@ -493,8 +493,8 @@ false roboglue_ros /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros - roboglue_ros_recorder - /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_recorder + roboglue_ros_broadcaster + /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_broadcaster 2 diff --git a/src/roboglue_ros/launch/roboglue_ros_urdriver.launch b/src/roboglue_ros/launch/roboglue_ros_urdriver.launch index 426d14e..b31d17e 100644 --- a/src/roboglue_ros/launch/roboglue_ros_urdriver.launch +++ b/src/roboglue_ros/launch/roboglue_ros_urdriver.launch @@ -69,6 +69,12 @@ + + + + + + diff --git a/src/roboglue_ros/src/roboglue_broadcaster.cpp b/src/roboglue_ros/src/roboglue_broadcaster.cpp index 32239dd..ac1344c 100644 --- a/src/roboglue_ros/src/roboglue_broadcaster.cpp +++ b/src/roboglue_ros/src/roboglue_broadcaster.cpp @@ -5,6 +5,7 @@ ////////// ROS INCLUDES ///////////// #include "ros/ros.h" #include /// utility function library +#include /////////////////////////////////////////// ///////// ROS CALLBACKS ////////////////// @@ -15,8 +16,25 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p nlohmann::json c = nlohmann::json::parse(msg->data.c_str()); std::string command = c["command"]; nlohmann::json params = c["params"]; + std::string action; if (!command.compare("QUIT")){ is->isRunning = false; + } 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"].get()/1000.0; + tempFrame.twist.linear.y = params["frame"]["y"].get()/1000.0; + tempFrame.twist.linear.z = params["frame"]["z"].get()/1000.0; + tempFrame.twist.angular.x = deg2rad(params["frame"]["rx"].get()); + tempFrame.twist.angular.y = deg2rad(params["frame"]["ry"].get()); + tempFrame.twist.angular.z = deg2rad(params["frame"]["rz"].get()); + pos->activeFrame = twist2Pose(tempFrame).pose; + } else; } else { ROS_WARN("Unknown command: %s", command.c_str()); } @@ -56,12 +74,17 @@ int main(int argc, char **argv) { positionData pos_data; robotData robot_data; auxData aux_data; + rvizData rviz_data; int tfPubRate; /// setup node parameters /// ros::init(argc, argv, "roboglue_broadcaster"); ros::NodeHandle nh; publisherMap publishers; + /// variables for TF publication + tf::TransformBroadcaster tfBroadcaster; + tf::Transform planningFrame; + tf::Quaternion frameRot; /// TODO: override default signal handler /// // signal(SIGINT, boost::bind(sigintHandler, _1, &is)); ///read parameters from server /// @@ -70,7 +93,9 @@ int main(int argc, char **argv) { // load robot model information 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+"jointHomePosition", robot_data.joint_homes); + 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 jog coord publish rate nh.getParam("/roboglue_ros_broadcaster/tf_pub_rate", tfPubRate); @@ -78,7 +103,7 @@ int main(int argc, char **argv) { ros::Rate loop_rate(common_par.loopRate); /// set console log level /// ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); - ROS_INFO("Follower Node Started"); + ROS_INFO("Broadcaster Node Started"); /// subscribe to incoming topics /// ros::Subscriber command_sub = nh.subscribe(common_par.ros_topics.commandSub, static_cast(common_par.msgBufferSize), @@ -118,6 +143,19 @@ int main(int argc, char **argv) { } ROS_DEBUG_ONCE("Broadcaster Node Looping"); ros::spinOnce(); + /// publish planning frame transform /// + + /// verify execution frame and set to default if needed + if (move_group.getPlanningFrame().compare(pos_data.activeFrameName)){ + ROS_DEBUG_ONCE("Planning Frame changed and publishing!"); + planningFrame.setOrigin(tf::Vector3(pos_data.activeFrame.position.x,pos_data.activeFrame.position.y,pos_data.activeFrame.position.z)); + frameRot.setRPY(pos_data.activeFrame.orientation.x,pos_data.activeFrame.orientation.y,pos_data.activeFrame.orientation.z); + planningFrame.setRotation(frameRot); + tfBroadcaster.sendTransform(tf::StampedTransform(planningFrame, ros::Time::now(), "world", "planning_frame")); + rviz_data.vtools->setBaseFrame("planning_frame"); + move_group.setPoseReferenceFrame("planning_frame"); + } + /// shutdown operations if(ros::isShuttingDown()){ declareShutdown(&publishers, &common_par); ros::spinOnce(); diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index 239b819..54ef2aa 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/src/roboglue_ros/src/roboglue_recorder.cpp @@ -88,22 +88,6 @@ 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"].get()/1000.0; - tempFrame.twist.linear.y = params["frame"]["y"].get()/1000.0; - tempFrame.twist.linear.z = params["frame"]["z"].get()/1000.0; - tempFrame.twist.angular.x = deg2rad(params["frame"]["rx"].get()); - tempFrame.twist.angular.y = deg2rad(params["frame"]["ry"].get()); - tempFrame.twist.angular.z = deg2rad(params["frame"]["rz"].get()); - pos->activeFrame = twist2Pose(tempFrame).pose; - } } else if (!command.compare("RECORD")){ action = params["action"]; if (!action.compare("start")){ @@ -352,22 +336,6 @@ int main(int argc, char **argv) { /////////// PLAY FILE /////////////////// ///////////////////////////////////////// - /// temporary variables for TF publication - tf::TransformBroadcaster tfBroadcaster; - tf::Transform planningFrame; - tf::Quaternion frameRot; - - // verify execution frame and set to default if needed - if (move_group.getPlanningFrame().compare(pos_data.activeFrameName)){ - ROS_DEBUG_ONCE("Planning Frame changed and publishing!"); - planningFrame.setOrigin(tf::Vector3(pos_data.activeFrame.position.x,pos_data.activeFrame.position.y,pos_data.activeFrame.position.z)); - frameRot.setRPY(pos_data.activeFrame.orientation.x,pos_data.activeFrame.orientation.y,pos_data.activeFrame.orientation.z); - planningFrame.setRotation(frameRot); - tfBroadcaster.sendTransform(tf::StampedTransform(planningFrame, ros::Time::now(), "world", "planning_frame")); - rviz_data.vtools->setBaseFrame("planning_frame"); - move_group.setPoseReferenceFrame("planning_frame"); - } - if (is.isPlaying){ // 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().c_str());