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

Aggiunto nodo Broadcaster continuo delle trasformazioni

Browse Source
This commit is contained in:
Emanuele
2019-11-14 16:10:36 +01:00
parent d725017879
commit e3b88fe742
3 changed files with 44 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/roboglue_ros/CMakeLists.txt
View File

@@ -13,7 +13,8 @@ find_package(catkin REQUIRED COMPONENTS
moveit_ros_planning
moveit_ros_planning_interface
moveit_visual_tools
)
tf
)
find_package (Eigen3 REQUIRED)
find_package (Boost REQUIRED system thread chrono)
@@ -148,6 +149,11 @@ include_directories(
src/roboglue_follower.cpp
src/roboglue_utils.cpp
)
add_executable(
${PROJECT_NAME}_broadcaster
src/roboglue_broadcaster.cpp
src/roboglue_utils.cpp
)
add_executable (
${PROJECT_NAME}_test
src/roboglue_test.cpp
@@ -177,6 +183,9 @@ target_link_libraries(
target_link_libraries(
${PROJECT_NAME}_follower ${catkin_LIBRARIES}
)
target_link_libraries(
${PROJECT_NAME}_broadcaster ${catkin_LIBRARIES}
)
#############

30
src/roboglue_ros/src/roboglue_broadcaster.cpp Normal file
View File

@@ -0,0 +1,30 @@
/*
20190220 - Inizia la scrittura del nodo che si occupa di pubblicate le trasformazioni TF, documentazione su GIT
*/
////////// ROS INCLUDES /////////////
#include "ros/ros.h"
#include <roboglue_ros/roboglue_utils.h> /// utility function library
int main(int argc, char **argv)
{
ros::init(argc, argv, "roboglue_broadcaster");
ros::NodeHandle nh;
ros::Publisher chatter_pub = nh.advertise<std_msgs::String>("chatter", 1000);
ros::Rate loop_rate(10);
while (ros::ok())
{
std_msgs::String msg;
msg.data = "hello world";
chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

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

@@ -99,9 +99,9 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
tempFrame.twist.linear.x = params["frame"]["x"].get<double>()/1000.0;
tempFrame.twist.linear.y = params["frame"]["y"].get<double>()/1000.0;
tempFrame.twist.linear.z = params["frame"]["z"].get<double>()/1000.0;
tempFrame.twist.angular.x = params["frame"]["rx"].get<double>()/1000.0;
tempFrame.twist.angular.y = params["frame"]["ry"].get<double>()/1000.0;
tempFrame.twist.angular.z = params["frame"]["rz"].get<double>()/1000.0;
tempFrame.twist.angular.x = deg2rad(params["frame"]["rx"].get<double>());
tempFrame.twist.angular.y = deg2rad(params["frame"]["ry"].get<double>());
tempFrame.twist.angular.z = deg2rad(params["frame"]["rz"].get<double>());
pos->activeFrame = twist2Pose(tempFrame).pose;
}
} else if (!command.compare("RECORD")){
@@ -362,12 +362,11 @@ int main(int argc, char **argv) {
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);
frameRot.setRPY(0,0,0);
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");
}
move_group.setPoseReferenceFrame("planning_frame");
if (is.isPlaying){
// i -> row counter for temp vector planning in both cartesian and joint mode