Skip to content

Commit

Permalink
Kinect TF + PS4 Relay
Browse files Browse the repository at this point in the history
  • Loading branch information
Anas Abou Allaban committed Jul 17, 2018
1 parent dd22161 commit 8599284
Show file tree
Hide file tree
Showing 6 changed files with 76 additions and 22 deletions.
17 changes: 9 additions & 8 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -8,11 +8,12 @@ project(jackal_bsc)
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
nav_msgs
message_filters
roscpp
rospy
geometry_msgs
message_filters
nav_msgs
tf
)

###################################
Expand Down Expand Up @@ -72,17 +73,17 @@ include_directories(
# ${catkin_LIBRARIES}
# )

add_executable(twist_stamped_publisher src/twist_stamped_publisher.cpp)
add_library(BSCTwistStamped src/BSCTwistStamped.cpp)
add_library(BSCSolver src/BSCSolver.cpp)

add_executable(twist_stamped_publisher src/twist_stamped_publisher.cpp)
target_link_libraries(twist_stamped_publisher BSCTwistStamped ${catkin_LIBRARIES})

add_executable(bsc_node src/bsc_node.cpp)
add_library(BSCSolver src/BSCSolver.cpp)

target_link_libraries(bsc_node BSCSolver ${catkin_LIBRARIES})

#add_executable(delay_node src/delay_node.cpp)
#target_link_libraries(delay_node ${catkin_LIBRARIES})
add_executable(kinect_tf_publisher src/kinect_tf_publisher.cpp)
target_link_libraries(kinect_tf_publisher ${catkin_LIBRARIES})

add_executable(drift_node src/drift_node.cpp)
target_link_libraries(drift_node ${catkin_LIBRARIES})
Expand Down
5 changes: 4 additions & 1 deletion launch/gmapping_kinect.launch
Original file line number Diff line number Diff line change
@@ -1,8 +1,11 @@
<launch>
<!-- Kinect TF -->
<include file="$(find jackal_bsc)/launch/kinect_tf.launch"/>

<!-- Fake laser scan-->
<arg name="scan_topic" default="/jackal_laser_scan"/>
<node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" type="depthimage_to_laserscan">
<remap from="image" to="/kinect2/hd/image_color"/>
<remap from="image" to="/kinect2/hd/image_depth_rect"/>
<remap from="camera_info" to="/kinect2/hd/camera_info"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>
Expand Down
3 changes: 3 additions & 0 deletions launch/kinect_tf.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
<launch>
<node name="kinect_tf_publisher" pkg="jackal_bsc" type="kinect_tf_publisher" output="screen"/>
</launch>
3 changes: 3 additions & 0 deletions launch/ps4_relay.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
<launch>
<node name="ps4_relay" pkg="topic_tools" type="relay" output="screen" args="/ps4_cmd_vel /cmd_vel"/>
</launch>
18 changes: 5 additions & 13 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -49,19 +49,11 @@
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>message_filters</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>message_filters</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>message_filters</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>

<depend>roscpp</depend>
<depend>rospy</depend>
<depend>geometry_msgs</depend>
<depend>message_filters</depend>
<depend>tf</depend>

<!-- The export tag contains other, unspecified, tags -->
<export>
Expand Down
52 changes: 52 additions & 0 deletions src/kinect_tf_publisher.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>


int main(int argc, char** argv) {
ros::init(argc, argv, "kinect_tf_publisher");

// Defs
static tf::TransformBroadcaster br;
std::vector<tf::StampedTransform> tf_vector;
tf::StampedTransform tf_front_mount_to_frame, tf_frame_to_ir, tf_frame_to_rgb;
tf::Quaternion q;
std::string front_mount = "front_mount";
std::string kinect_link = "kinect2_link";
std::string rgb_link = "kinect2_rgb_optical_frame";
std::string ir_link = "kinect2_ir_optical_frame";

// Default rotation
q.setRPY(0, 0, 0);

// Translations
tf_front_mount_to_frame.setOrigin(tf::Vector3(0.174, 0, 0.2));
tf_front_mount_to_frame.setRotation(q);
tf_front_mount_to_frame.frame_id_ = front_mount;
tf_front_mount_to_frame.child_frame_id_ = kinect_link;

tf_frame_to_rgb.setOrigin(tf::Vector3(0, -0.1, 0));
tf_frame_to_rgb.setRotation(q);
tf_frame_to_rgb.frame_id_= kinect_link;
tf_frame_to_rgb.child_frame_id_ = rgb_link;

tf_frame_to_ir.setOrigin(tf::Vector3(0, -0.06, 0));
tf_frame_to_ir.setRotation(q);
tf_frame_to_ir.frame_id_ = kinect_link;
tf_frame_to_ir.child_frame_id_ = ir_link;

ROS_INFO("[Kinect TF]: Broadcasting...");
ros::Rate r(5);
while (ros::ok()) {
tf_vector.clear();
tf_front_mount_to_frame.stamp_ = ros::Time::now();
tf_frame_to_rgb.stamp_ = ros::Time::now();
tf_frame_to_ir.stamp_ = ros::Time::now();
tf_vector.push_back(tf_front_mount_to_frame);
tf_vector.push_back(tf_frame_to_ir);
tf_vector.push_back(tf_frame_to_rgb);
br.sendTransform(tf_vector);
r.sleep();
}

return 0;
}

0 comments on commit 8599284

Please sign in to comment.