RSA_kustar's profile - activity

I have two robot models. The first robot is the vehicle (AR.Drone) and the second is the Phantom Omni Device. I use rviz for visualization. I have problem with the tree of the TFs. Now for the AR.Drone, the parent frame is the 'world' frame while the parent frame for the Phantom Omni is the base frame. Now there are two problems. The first one is that when the TFs are shown in Rviz, the world frame appear for 3 seconds and starts to disappear. The second problem, the base TF is not shown. I want both robots to be shown with their URDF files but RViz only allow us to choose one Fixed frame.. What should I do ?

Hi,

I created a service with the following definition and i called it navigation.srv:

float64 srvtype
float64 wayPointX
float64 wayPointY
float64 wayPointZ
float64 LDFlag
---
std_msgs/String navResponse

Then I used the service state machine provided from smach and following the tutorials:

import roslib;  
import rospy
import actionlib 
import smach
import smach_ros
import time
from std_msgs.msg import String
from sensor_msgs.msg import RegionOfInterest
from geometry_msgs.msg import PoseStamped 
from kuri_msgs.srv import navigation

from kuri_msgs.msg import Object
from sensor_msgs.msg import NavSatFix
from nav_msgs.msg import Odometry
from smach_ros import ServiceState

 FixPoseX = 0   
 FixPoseY = 0 
 FixPoseZ = 10  


class Ch1():
 rospy.init_node('ch1_state_machine')
 sm = smach.StateMachine(outcomes=['Done'])

 with sm:

    def exploration_request_cb (userdata,request):
        exploration_request = navigation().Request  
        exploration_request.srvtype=1
        exploration_request.wayPointX = FixPoseX
        exploration_request.wayPointY = FixPoseY
        exploration_request.wayPointZ = FixPoseZ
        exploration_request.LDFlag = 0
        return exploration_request



        smach.StateMachine.add('EXPLORATION',
                ServiceState('controller_service',
                navigation,
                request  = exploration_request_cb),
                       transitions={'succeeded':'MARKER_DETECTION' , 'aborted':'Done', 'preempted':'EXPLORATION'})

    ### MORE STATES HERE

 sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
     sis.start()

     # Execute SMACH plan
     outcome = sm.execute()
     rospy.spin()
     sis.stop()


if __name__ == '__main__':
   Ch1()

The Service the I am using to be called ( I am also having a problem in sending back a response is as follow)

when I run the state machine it says that it is waiting for the service then I run the service the output is the following:

 import roslib
 import rospy
 from kuri_msgs.msg import *
 from kuri_msgs.srv import navigation
 from kuri_msgs.srv import navigationResponse
 from geometry_msgs.msg import PoseStamped 
 from nav_msgs.msg import Odometry 

 class ControllerService:
         def __init__(self):
               print 'Starting Service 1'
               self.s = rospy.Service('controller_service', navigation , self.pos_vel_controller)   

        def pos_vel_controller(self , req):
            self.landFlagDone = req.LDFlag 
                if req.srvtype == 1 :
                         x = req.wayPointX
             y  = req.wayPointY
                 z = req.wayPointZ
             if self.landFlagDone >0:
#######################3 How to return a response here ???????????????????
                 return 'succeeded'
             else: 
                    return 'aborted'


def main(args):
  rospy.init_node('Controller_Service')
  Controller = ControllerService()
  try:
     rospy.spin()
 except KeyboardInterrupt:
    print "Shutting down"

if __name__ == '__main__':
    main(sys.argv)

In the service terminal Killed

In the state machine terminal

 TypeError: Invalid number of arguments, args should be ['srvtype', 'wayPointX', 'wayPointY', 'wayPointZ', 'LDFlag'] args are(<function exploration_request_cb at 0x7f0f3fae6140>,)

What is the mistake in this code and how can I fix it ?

Writing map.ot file generates lines with the same adress Hi, I am trying to save the octomap in ".ot" format and visua

Writing map.ot file generates lines with the same adress Hi, I am trying to save the octomap in ".ot" format and visua

@TharushiDeSilva No I can't visualize it using octovis. I got a msg "cannot open Octree file"

@TharushiDeSilva I am using /octomap_full topic and the voxel coloring is the cell color but still all voxels are white

@TharushiDeSilva I am using /octomap_full topic and the voxel coloring is the cell color but still all voxels are white

@TharushiDeSilva I am using /octomap_full topic and the voxel coloring is the cell color but still all voxels are white

ColorOctree readData and writeData functions' visualization in RVIZ shows white voxels all the time Hi, I am creating

readData and writeData function for ColorOctree visualization is giving white color in RVIZ els Hi, I am creating a co

Hello,

I know that this question might be asked before, but I did not find the final solution for it. Here is the question:

I want to write a node that subscribe for data from a topic then publish these data to another topic. So, mainly I want a publisher and subscriber in the same node. So What I did so far is the following:

#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include "pctx_control/Control.h"
#include "std_msgs/String.h"
#include "pctx_control/pctx.h" 
#include <sstream>


pctx_control::Control controlmsg ;

void function_callback (const geometry_msgs::Twist &msg) 
 {

controlmsg.ch = 0 ;
controlmsg.values[0] = msg.linear.x;
controlmsg.values[1] = msg.linear.y;
controlmsg.values[2] = msg.linear.z;
std::cout << "d1"<<controlmsg.values[0] << std::endl ;
std::cout << "d2" <<controlmsg.values[1]<< std::endl ;
std::cout << "d3" <<controlmsg.values[2]<< std::endl ;

}

int main(int argc, char **argv)
{

 ros::init(argc, argv, "my_node");
  ros::NodeHandle n;
 ros::Publisher pub = n.advertise<pctx_control::Control>("topic2", 100);

  ros::Subscriber sub = n.subscribe("topic1", 1000, function_callback);

 ros::Rate loop_rate(10);
 ros::spinOnce();

 int count = 0;
 while (ros::ok())
  {

    pub.publish(controlmsg);

  ros::spin();
  loop_rate.sleep();

 }

return 0;
 }

I want to save data that I want to publish in a "custom msg" pctx_control::Control type;

This msg has the following format:

int32 ch
int32 value
int16[] values

I am using a pioneer3AT with a Hykuoy laser finder. when I run the RosAria, I have 2 frames The odom frame ( fixed frame wich represent the start of the movement) and the baselink frame which is attached to the body of the robot.

Also, when I run the URG_node I get another frame which is the laser frame.

I added the laser in the same direction of the base_link and I calculated the distance from the base_link frame to the laser frame is [0.206 0 0.16] in meters and I created a static tf transformation between the laser and the baselink as following.

  <launch>
  <node pkg="tf" type="static_transform_publisher" name="laser_frame" args="0.206 0 0.16 0 0 0 1 base_link laser 100" />
 </launch>

My TF tree is as follows: odom -> base_link -> laser

now, in the code, I am reading the sensor_msgs/laserScan msgs as (r, theta) -->

What I want is to do transformation these data from laser frame to odom frame ?? how can I do that in ROS (in c++)??

I have two objects. The first object is my robot which I want to represent it as a shpare and the second object is the obstacle that has unkonwn shape. I want to represent the shape of the obstacle with octree.

how can i use the api of the fcl to check collision between these two objects ( true or false) usinf fcl libraries using the api form ROS wiki? giving that the robot is moving.

Also, the obstacle is detected using laser scan data?? how to fill it in octree object ??

EDIT:

I wrote the following code but I dont know how to fill the octree

boost::shared_ptr<Sphere> Shpere0(new Sphere(1));
OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
// GJKSolver_indep solver;
GJKSolver_libccd solver;
Vec3f contact_points;
FCL_REAL penetration_depth;
Vec3f normal;
Transform3f tf0, tf1;
tf0.setIdentity();
tf0.setTranslation(Vec3f(robotpose(0),robotpose(1),robotpose(2)));
tf0.setQuatRotation(Quaternion3f(0, 0, 0, 0));

 // HOW TO FILL the OCTREE HERE with point cloud data ???  
  tf1.setIdentity();
 bool res = solver.shapeIntersect(*Shpere0, tf0, *box1, tf1, &contact_points, &penetration_depth, &normal);
 cout << "contact points: " << contact_points << endl;
 cout << "pen depth: " << penetration_depth << endl;
 cout << "normal: " << normal << endl;
 cout << "result: " << res << endl;
 static const int num_max_contacts = std::numeric_limits<int>::max();
 static const bool enable_contact = true;
 fcl::CollisionResult result;
 fcl::CollisionRequest request(num_max_contacts, enable_contact);
 CollisionObject co0(Shpere0, tf0);
 CollisionObject co1(tree, tf1);
bool res_1 =   fcl::collide(&co0, &co1, request, result);

??

How can I fill the octree or octmap to be presented as an object and I can use the function collied to check if the shapre ( which represent my robot) is going to collied with the octree or octmap that I should construct from a point cloud data ?

I want to do a static transformation

the actual frame is in x forward y right z up the new frame if x forward y left and z up

I want to do transformation from the new to the actual frame using the static publish

<launch>

 <node pkg="tf" type="static_transform_publisher" name="laser" args="0 0 0 x y z w  /uav/base_link_ENU /laser  100"/>

</launch>

What should I replace the x y z and w
And do I need to have a listener ??

I have the following launch file:

<?xml version="1.0"?>

<launch>
    <!--<include file="$(find husky_gazebo)/launch/base.urdf.gazebo.launch"/>-->
     <param name="use_sim_time" value="true" />

    <!-- Send the Husky A200 Robot URDF/XACRO to param server -->
    <param name="robot_description" command="$(find xacro)/xacro.py '$(find husky_description)/urdf/base.urdf.xacro'" />

    <!-- Spawn robot in gazebo -->
    <node name="spawn_husky_model" pkg="gazebo_ros"  type="spawn_model" args="$(optenv ROBOT_INITIAL_POSE) -unpause -urdf -param robot_description -model mobile_base />

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" ns="husky_ns">
        <param name="publish_frequency" type="double" value="50.0" />
    </node>




    <include file="$(find gazebo_ros)/launch/empty_world.launch"/>
    <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" ns="husky_ns" >
      <rosparam>
        freq: 10.0
        sensor_timeout: 1.0
        publish_tf: true
        odom_used: true
        vo_used: true
        output_frame: odom
      </rosparam>
    </node>
</launch>

I want to add a name space to this node but I cant

 <!-- Spawn robot in gazebo -->
    <node name="spawn_husky_model" pkg="gazebo_ros"  type="spawn_model" args="$(optenv ROBOT_INITIAL_POSE) -unpause -urdf -param robot_description -model mobile_base ns="husky_ns" />

Why I cant add ? and how can I add a name space for this node ?