--- /dev/null
+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)
--- /dev/null
+bool[9] input
+int16 d1
--- /dev/null
+int32 num
+int32 speed
--- /dev/null
+int32 num
+int32 speed
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>roboint</name>
+ <version>0.0.0</version>
+ <description>The roboint package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="erik@vontaene.de">Erik Andresen</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>BSD</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://ros.org/wiki/roboint</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>
+ <run_depend>roscpp</run_depend>
+ <run_depend>rospy</run_depend>
+ <run_depend>std_msgs</run_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>nav_msgs</build_depend>
+ <build_depend>tf</build_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>
--- /dev/null
+#!/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()
--- /dev/null
+#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<roboint::Inputs>("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;
+}
+
projects / ros_roboint.git / commitdiff