Commit 78464b8d by Ryan Keating

Fixed ur5 package

parent 6c7aeba8
cmake_minimum_required(VERSION 2.8.3)
project(ur5)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
visualization_msgs
trajectory_msgs
message_generation
tf
)
find_package(Eigen REQUIRED)
find_package(Boost REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependencies might have been
## pulled in transitively but can be declared for certainty nonetheless:
## * add a build_depend tag for "message_generation"
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
ur5_joints.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs# trajectory_msgs
)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES ur5
# CATKIN_DEPENDS roscpp rospy std_msgs trajectory_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
include
${catkin_INCLUDE_DIRS}
${Eigen_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
)
## Declare a cpp library
add_library(ur5 src/utilities.cpp)
target_link_libraries(ur5 ${catkin_LIBRARIES})
## Declare a cpp executable
add_executable(arm_controller_commander src/arm_controller_commander.cpp)
add_executable(rviz_joint_state_publisher src/rviz_joint_state_publisher.cpp)
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
# add_dependencies(ur5_node ur5_generate_messages_cpp)
## Specify libraries to link a library or executable target against
target_link_libraries(arm_controller_commander
${catkin_LIBRARIES}
)
target_link_libraries(rviz_joint_state_publisher
${catkin_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ur5 ur5_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ur5.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
#ifndef UTILITIES_H
#define UTILITIES_H
#include <Eigen/Core>
#include <tf/transform_broadcaster.h>
#include <boost/thread.hpp>
#include <visualization_msgs/Marker.h>
class RvizPlotter{
class RvizFrame{
public:
std::string parentName;
std::string childName;
tf::Transform transform;
RvizFrame(tf::Transform transform, std::string parentName, std::string childName);
};
std::vector<RvizFrame> frames;
std::vector<visualization_msgs::Marker> vectors;
tf::TransformBroadcaster br;
ros::Publisher pb;
boost::thread *broadcastThread;
void broadcast();
public:
RvizPlotter();
RvizPlotter(ros::NodeHandle &n);
~RvizPlotter();
void plotf(Eigen::Matrix4f f, std::string parentName, std::string childName);
void plotf(Eigen::Matrix4f f, std::string frameName);
void plotv(std::string frameName, Eigen::Vector3f point1, Eigen::Vector3f point2);
};
void printEigen(const Eigen::MatrixXf& m);
Eigen::Matrix4f getTransformation(std::string parentName, std::string childName);
#endif
\ No newline at end of file
<?xml version="1.0"?>
<launch>
<!-- startup simulated world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" default="worlds/empty.world"/>
</include>
<param name="robot_description" command="$(find xacro)/xacro.py '$(find ur5)/urdf/gazebo/ur5.xacro'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />
<include file="$(find ur_gazebo)/launch/controller_utils.launch"/>
<rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/>
<node name="arm_controller_spawner" pkg="controller_manager" type="spawner" args="arm_controller" />
</launch>
<launch>
<param name="robot_description" textfile="$(find ur5)/urdf/rviz/ur5.urdf" />
<param name="use_gui" value="True"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ur5)/launch/urdf.rviz" required="true" />
</launch>
<launch>
<param name="robot_description" textfile="$(find ur5)/urdf/rviz/ur5.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>
<node name="rviz_joint_state_publisher" pkg="ur5" type="rviz_joint_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ur5)/launch/urdf.rviz" required="true" />
</launch>
Panels:
- Class: rviz/Displays
Help Height: 91
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1
- /TF1
- /TF1/Frames1
Splitter Ratio: 0.667742
Tree Height: 1435
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 0.75
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
ee_link:
Alpha: 1
Show Axes: false
Show Trail: false
forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
shoulder_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 1e+06
Frames:
All Enabled: false
base_link:
Value: true
ee_link:
Value: false
forearm:
Value: true
forearm_link:
Value: false
shoulder:
Value: true
shoulder_link:
Value: false
upper_arm:
Value: true
upper_arm_link:
Value: false
wrist_1:
Value: true
wrist_1_link:
Value: false
wrist_2:
Value: true
wrist_2_link:
Value: false
wrist_3:
Value: true
wrist_3_link:
Value: false
Marker Scale: 0.25
Name: TF
Show Arrows: false
Show Axes: true
Show Names: false
Tree:
base_link:
forearm:
{}
shoulder:
{}
shoulder_link:
upper_arm_link:
forearm_link:
wrist_1_link:
wrist_2_link:
wrist_3_link:
ee_link:
{}
upper_arm:
{}
wrist_1:
{}
wrist_2:
{}
wrist_3:
{}
Update Interval: 0
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: base_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 1.97589
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.337396
Y: -0.0844933
Z: -0.0591265
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.0347947
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 1.60674
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1776
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000013c0000064afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007a00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000064a0000010a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000012d0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000e200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000aef0000004efc0100000002fb0000000800540069006d0065010000000000000aef0000043f00fffffffb0000000800540069006d00650100000000000004500000000000000000000009ad0000064a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 2799
X: 81
Y: 24
float64[] q
\ No newline at end of file
<?xml version="1.0"?>
<package>
<name>ur5</name>
<version>0.0.0</version>
<description>The ur5 package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="rkeatin3@todo.todo">rkeatin3</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/ur5</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>trajectory_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
</export>
</package>
\ No newline at end of file
#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <std_msgs/Bool.h>
ros::Publisher pb;
trajectory_msgs::JointTrajectory j;
void goCallback(const std_msgs::Bool& b)
{
pb.publish(j);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "arm_controller_commander");
ros::NodeHandle n;
pb = n.advertise<trajectory_msgs::JointTrajectory>("arm_controller/command", 10);
j.header.stamp = ros::Time::now();
j.header.seq = 1;
j.joint_names.push_back("shoulder_pan_joint");
j.joint_names.push_back("shoulder_lift_joint");
j.joint_names.push_back("elbow_joint");
j.joint_names.push_back("wrist_1_joint");
j.joint_names.push_back("wrist_2_joint");
j.joint_names.push_back("wrist_3_joint");
trajectory_msgs::JointTrajectoryPoint p1;
p1.positions.push_back(1.57075);
p1.positions.push_back(1.57075);
p1.positions.push_back(1.57075);
p1.positions.push_back(1.57075);
p1.positions.push_back(1.57075);
p1.positions.push_back(1.57075);
p1.velocities.push_back(.25);
p1.velocities.push_back(.25);
p1.velocities.push_back(.25);
p1.velocities.push_back(.25);
p1.velocities.push_back(.25);
p1.velocities.push_back(.25);
p1.time_from_start = ros::Duration(20);
trajectory_msgs::JointTrajectoryPoint p2;
p2.positions.push_back(-1.57075);
p2.positions.push_back(-1.57075);
p2.positions.push_back(-1.57075);
p2.positions.push_back(-1.57075);
p2.positions.push_back(-1.57075);
p2.positions.push_back(-1.57075);
p2.velocities.push_back(-.25);
p2.velocities.push_back(-.25);
p2.velocities.push_back(-.25);
p2.velocities.push_back(-.25);
p2.velocities.push_back(-.25);
p2.velocities.push_back(-.25);
p2.time_from_start = ros::Duration(40);
j.points.push_back(p1);
j.points.push_back(p2);
ros::Subscriber sb = n.subscribe("go", 10, &goCallback);
while (n.ok()){
ros::spinOnce();
}
}
\ No newline at end of file
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <ur5/ur5_joints.h>
#include <tf/transform_listener.h>
ros::Publisher pb;
sensor_msgs::JointState j;
void forwardKinematicsCallback(const ur5::ur5_joints& joints)
{
j.position.clear();
j.position.push_back(joints.q[0]);
j.position.push_back(joints.q[1]);
j.position.push_back(joints.q[2]);
j.position.push_back(joints.q[3]);
j.position.push_back(joints.q[4]);
j.position.push_back(joints.q[5]);
pb.publish(j);
}
// void InverseKinematicsCallback(const std_msgs::Bool& b)
// {
// }
int main(int argc, char **argv)
{
ros::init(argc, argv, "rviz_joint_state_publisher");
ros::NodeHandle n;
pb = n.advertise<sensor_msgs::JointState>("/joint_states", 10);
ros::Subscriber forwardSub = n.subscribe("forwardKinematics", 10, &forwardKinematicsCallback);
ros::Rate rate(100);
j.header.stamp = ros::Time::now();
j.header.seq = 1;
j.name.push_back("shoulder_pan_joint");
j.name.push_back("shoulder_lift_joint");
j.name.push_back("elbow_joint");
j.name.push_back("wrist_1_joint");
j.name.push_back("wrist_2_joint");
j.name.push_back("wrist_3_joint");
j.position.push_back(0.0);
j.position.push_back(0.0);
j.position.push_back(0.0);
j.position.push_back(0.0);
j.position.push_back(0.0);
j.position.push_back(0.0);
pb.publish(j);
while(ros::ok())
{
//spin
ros::spinOnce();
j.header.stamp = ros::Time::now();
j.header.seq += 1;
pb.publish(j);
rate.sleep();
}
}
\ No newline at end of file
#include <ros/ros.h>
#include <Eigen/Core>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <ur5/utilities.h>
RvizPlotter::RvizFrame::RvizFrame(tf::Transform transform, std::string parentName, std::string childName){
this->transform = transform;
this->parentName = parentName;
this->childName = childName;
}
void RvizPlotter::plotf(Eigen::Matrix4f h, std::string parentName, std::string childName){
tf::Transform transform;
transform.setOrigin(tf::Vector3(h(0,3), h(1,3), h(2,3)));
transform.setBasis(tf::Matrix3x3(h(0,0), h(0,1), h(0,2),
h(1,0), h(1,1), h(1,2),
h(2,0), h(2,1), h(2,2)));
for(int i=0; i < frames.size(); i++)
{
if(childName.compare(frames[i].childName.c_str()) == 0)
{
frames[i].parentName = parentName;
frames[i].transform = transform;
usleep(50000);
return;
}
}
frames.push_back(RvizPlotter::RvizFrame(transform, parentName, childName));
usleep(50000);
}
void RvizPlotter::plotf(Eigen::Matrix4f h, std::string frameName){
plotf(h,"map",frameName);
}
void RvizPlotter::plotv(std::string frameName, Eigen::Vector3f start, Eigen::Vector3f end)
{
frameName.insert(0,"/");
visualization_msgs::Marker marker;
marker.header.frame_id = frameName;
marker.header.stamp = ros::Time();
marker.ns = "vectors";
marker.id = vectors.size();
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.4;
geometry_msgs::Point point1;
geometry_msgs::Point point2;
point1.x = start(0);
point1.y = start(1);
point1.z = start(2);
point2.x = end(0);
point2.y = end(1);
point2.z = end(2);