From e3560f756faac781896e0402caca83d330eee1a4 Mon Sep 17 00:00:00 2001 From: erik Date: Wed, 1 May 2013 10:45:08 -0400 Subject: [PATCH] Initial commit --- CMakeLists.txt | 133 ++++++++++++++++++++++++++++++++++++++ msg/Inputs.msg | 2 + msg/Motor.msg | 2 + msg/Output.msg | 2 + package.xml | 62 ++++++++++++++++++ scripts/robo_explorer.py | 133 ++++++++++++++++++++++++++++++++++++++ src/libft_adapter.cpp | 134 +++++++++++++++++++++++++++++++++++++++ 7 files changed, 468 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 msg/Inputs.msg create mode 100644 msg/Motor.msg create mode 100644 msg/Output.msg create mode 100644 package.xml create mode 100755 scripts/robo_explorer.py create mode 100644 src/libft_adapter.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..16e33b2 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,133 @@ +cmake_minimum_required(VERSION 2.8.3) +project(roboint) + +## 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 message_generation) + +## 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/groovy/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +####################################### +## Declare ROS messages and services ## +####################################### + +## Generate messages in the 'msg' folder +add_message_files( + FILES + Inputs.msg + Motor.msg + Output.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## 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 roboint + CATKIN_DEPENDS message_runtime roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(include + ${catkin_INCLUDE_DIRS} +) + +## Declare a cpp library +# add_library(roboint +# src/${PROJECT_NAME}/roboint.cpp +# ) + +## Declare a cpp executable +# add_executable(roboint_node src/roboint_node.cpp) +add_executable(libft_adapter src/libft_adapter.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +add_dependencies(libft_adapter roboint_gencpp) + +## Specify libraries to link a library or executable target against +# target_link_libraries(roboint_node +# ${catkin_LIBRARIES} +# ) +target_link_libraries(libft_adapter ${catkin_LIBRARIES} roboint) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/groovy/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 roboint roboint_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_roboint.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) diff --git a/msg/Inputs.msg b/msg/Inputs.msg new file mode 100644 index 0000000..86240c3 --- /dev/null +++ b/msg/Inputs.msg @@ -0,0 +1,2 @@ +bool[9] input +int16 d1 diff --git a/msg/Motor.msg b/msg/Motor.msg new file mode 100644 index 0000000..a2d44d9 --- /dev/null +++ b/msg/Motor.msg @@ -0,0 +1,2 @@ +int32 num +int32 speed diff --git a/msg/Output.msg b/msg/Output.msg new file mode 100644 index 0000000..a2d44d9 --- /dev/null +++ b/msg/Output.msg @@ -0,0 +1,2 @@ +int32 num +int32 speed diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..7aac638 --- /dev/null +++ b/package.xml @@ -0,0 +1,62 @@ + + + roboint + 0.0.0 + The roboint package + + + + + Erik Andresen + + + + + + BSD + + + + + + + + + + + + + + + + + + + message_generation + + + + message_runtime + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + geometry_msgs + sensor_msgs + nav_msgs + tf + + + + + + + + + + diff --git a/scripts/robo_explorer.py b/scripts/robo_explorer.py new file mode 100755 index 0000000..67c9963 --- /dev/null +++ b/scripts/robo_explorer.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python +import roslib; roslib.load_manifest('roboint') +import rospy +import tf +from math import sin, cos +from geometry_msgs.msg import Twist, TransformStamped, Point32 +from sensor_msgs.msg import PointCloud +from nav_msgs.msg import Odometry +from roboint.msg import Motor +from roboint.msg import Inputs + + +class RoboExplorer: + def __init__(self): + rospy.init_node('robo_explorer') + + rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived) + rospy.Subscriber("ft/get_inputs", Inputs, self.inputsReceived) + + self.pub_motor = rospy.Publisher("ft/set_motor", Motor) + self.pub_cloud = rospy.Publisher("point_cloud", PointCloud) + self.pub_odom = rospy.Publisher("odom", Odometry) + + self.speed = (0, 0) + self.x = 0 + self.y = 0 + self.th = 0 + self.last_in = [0, 0] + self.odom_broadcaster = tf.TransformBroadcaster() + self.last_time = rospy.Time.now() + + rospy.spin() + + def inputsReceived(self, msg): + current_time = rospy.Time.now() + + cloud = PointCloud() + cloud.header.stamp = current_time + cloud.header.frame_id = "sensor_frame" + cloud.points.append(Point32(msg.d1/10.0, 0, 0)) + self.pub_cloud.publish(cloud) + + dt = (current_time - self.last_time).to_sec(); + in_now = msg.input[1:3] + in_diff = [abs(a - b) for a, b in zip(in_now, self.last_in)] # get changed inputs + if self.speed[0] < 0: + in_diff[0] = -in_diff[0] + if self.speed[1] < 0: + in_diff[1] = -in_diff[1] + + self.diff_to_angle = 0.1 # TODO + diff_si = in_diff # TODO + + delta_th = (diff_si[0] - diff_si[1]) * self.diff_to_angle + self.th += delta_th + + movement = (diff_si[0] + diff_si[1])/2.0 + delta_x = cos(self.th)*movement + delta_y = sin(self.th)*movement + self.x += delta_x + self.y += delta_y + + # speeds + vx = delta_x / dt + vy = delta_y / dt + vth = delta_th / dt + + # first, we'll publish the transform over tf + odom_trans = TransformStamped() + odom_trans.header.stamp = current_time + odom_trans.header.frame_id = "odom" + odom_trans.child_frame_id = "base_link" + odom_trans.transform.translation.x = self.x + odom_trans.transform.translation.y = self.y + odom_trans.transform.translation.z = 0.0 + odom_trans.transform.rotation = self.th + + ## send the transform + #self.odom_broadcaster.sendTransform(odom_trans); + + # next, we'll publish the odometry message over ROS + odom = Odometry() + odom.header.stamp = current_time + odom.header.frame_id = "odom" + + # set the position + odom.pose.pose.position.x = self.x + odom.pose.pose.position.y = self.y + odom.pose.pose.position.z = 0.0 + odom.pose.pose.orientation = self.th + + # set the velocity + odom.child_frame_id = "base_link"; + odom.twist.twist.linear.x = vx + odom.twist.twist.linear.y = vy + odom.twist.twist.angular.z = vth + + # publish the message + self.pub_odom.publish(odom) + + self.last_time = current_time + self.last_in = in_now + + + def cmdVelReceived(self, msg): + trans = msg.linear.x + rot = msg.angular.z + + # speed steps = 7 + # max trans = 0.1m/s + # max rot = 0.29rad/s + + speed_l = int(trans*7/0.1 - rot*7/0.29) + speed_r = int(trans*7/0.1 + rot*7/0.29) + if speed_l < -7: speed_l = -7 + elif speed_l > 7: speed_l = 7 + if speed_r < -7: speed_r = -7 + elif speed_r > 7: speed_r = 7 + + outmsg = Motor() + outmsg.num = 1 + outmsg.speed = speed_l + self.pub_motor.publish(outmsg) + + outmsg = Motor() + outmsg.num = 2 + outmsg.speed = speed_r + self.pub_motor.publish(outmsg) + + self.speed = (speed_l, speed_r) + +if __name__ == '__main__': + RoboExplorer() diff --git a/src/libft_adapter.cpp b/src/libft_adapter.cpp new file mode 100644 index 0000000..9247894 --- /dev/null +++ b/src/libft_adapter.cpp @@ -0,0 +1,134 @@ +#include "roboint.h" +#include "ros/ros.h" +#include "roboint/Output.h" +#include "roboint/Motor.h" +#include "roboint/Inputs.h" + + +FT_TRANSFER_AREA *transfer_area = NULL; + + +void cb_set_output(const ::roboint::OutputConstPtr& msg) { + if (msg->speed == 0) { + transfer_area->M_Main &= ~(1<<(msg->num-1)); + } else { + transfer_area->M_Main |= (1<<(msg->num-1)); + } + transfer_area->MPWM_Main[msg->num-1] = msg->speed; +} + + +void cb_set_motor(const ::roboint::MotorConstPtr& msg) { + unsigned char iDirection = 0; + + if (msg->speed > 0) iDirection = 0x1; + else if (msg->speed < 0) iDirection = 0x2; + + transfer_area->M_Main &= ~(3<<(msg->num-1)*2); + transfer_area->M_Main |= iDirection<<(msg->num-1)*2; + transfer_area->MPWM_Main[(msg->num-1)*2] = msg->speed; + transfer_area->MPWM_Main[(msg->num-1)*2 + 1] = msg->speed; +} + + +int main(int argc, char **argv) +{ + FT_HANDLE hFt = NULL; + + InitFtLib(); + + // Get List of USB devices + InitFtUsbDeviceList(); + + // Get handle on a device + if ((hFt = GetFtUsbDeviceHandle(0)) == NULL) { + fprintf(stderr, "No ft Device found\n"); + return 1; + } + + // Open connection + OpenFtUsbDevice(hFt); + SetFtDistanceSensorMode(hFt, IF_DS_INPUT_DISTANCE, IF_DS_INPUT_TOL_STD, IF_DS_INPUT_TOL_STD, 100, 100, IF_DS_INPUT_REP_STD, IF_DS_INPUT_REP_STD); + + StartFtTransferArea(hFt, NULL); + transfer_area = GetFtTransferAreaAddress(hFt); + + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. For programmatic + * remappings you can use a different version of init() which takes remappings + * directly, but for most command-line programs, passing argc and argv is the easiest + * way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of ros::init() before using any other + * part of the ROS system. + */ + ros::init(argc, argv, "libft_adapter"); + + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ + ros::NodeHandle n; + + ros::Subscriber sub_set_output = n.subscribe("ft/set_output", 8, cb_set_output); + ros::Subscriber sub_set_motor = n.subscribe("ft/set_motor", 4, cb_set_motor); + + /** + * The advertise() function is how you tell ROS that you want to + * publish on a given topic name. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. After this advertise() call is made, the master + * node will notify anyone who is trying to subscribe to this topic name, + * and they will in turn negotiate a peer-to-peer connection with this + * node. advertise() returns a Publisher object which allows you to + * publish messages on that topic through a call to publish(). Once + * all copies of the returned Publisher object are destroyed, the topic + * will be automatically unadvertised. + * + * The second parameter to advertise() is the size of the message queue + * used for publishing messages. If messages are published more quickly + * than we can send them, the number here specifies how many messages to + * buffer up before throwing some away. + */ + ros::Publisher chatter_pub = n.advertise("ft/get_inputs", 1000); + ros::Rate loop_rate(5); + + while(ros::ok()) { + roboint::Inputs msg; + + msg.input[0] = 0; // unused, Hardware starts at 1 + for (int i=1; i<=8; i++) { + msg.input[i] = (transfer_area->E_Main & (1<<(i-1))) >> (i-1); + } + msg.d1 = transfer_area->D1; + + /** + * The publish() function is how you send messages. The parameter + * is the message object. The type of this object must agree with the type + * given as a template parameter to the advertise<>() call, as was done + * in the constructor above. + */ + chatter_pub.publish(msg); + + /** + * ros::spin() will enter a loop, pumping callbacks. With this version, all + * callbacks will be called from within this thread (the main one). ros::spin() + * will exit when Ctrl-C is pressed, or the node is shutdown by the master. + */ + ros::spinOnce(); + + loop_rate.sleep(); + } + + StopFtTransferArea(hFt); + + // Close connection + CloseFtDevice(hFt); + + CloseFtLib(); + + return 0; +} + -- 2.39.5